From 5a384ae0ffaadc4d4333e49a605ad7505bdda72a Mon Sep 17 00:00:00 2001 From: Tyler Roman Date: Sat, 13 Apr 2024 17:13:12 -0500 Subject: [PATCH 01/29] =?UTF-8?q?BIGGEST=20rover=20commit=20=F0=9F=98=AD?= =?UTF-8?q?=EF=B8=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 164 ++++++++++++++ config.ini | 4 +- examples/location.py | 8 +- examples/wheels.py | 15 +- libs/ARTracker.py | 509 ++++++++++++++++++++++++++----------------- libs/Drive.py | 460 +++++++++++++++++++------------------- libs/Location.py | 77 ++++--- libs/Locationf9p.py | 71 +++--- libs/UDPOut.py | 65 +++--- main.py | 221 +++++++++++-------- 10 files changed, 973 insertions(+), 621 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e76787d --- /dev/null +++ b/.gitignore @@ -0,0 +1,164 @@ +# Ignore vim .swp files +**.swp +**.swo + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/#use-with-ide +.pdm.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/build/ \ No newline at end of file diff --git a/config.ini b/config.ini index 840201f..b5d724f 100644 --- a/config.ini +++ b/config.ini @@ -1,7 +1,7 @@ [CONFIG] -SWIFT_IP=10.0.0.222 +SWIFT_IP=192.168.1.222 SWIFT_PORT=55556 -MBED_IP=10.0.0.101 +MBED_IP=192.168.1.101 MBED_PORT=1001 [ARTRACKER] #dpp is .040625 with logi diff --git a/examples/location.py b/examples/location.py index 2fdc518..001c9bb 100644 --- a/examples/location.py +++ b/examples/location.py @@ -3,14 +3,14 @@ #os.chdir(os.path.dirname(os.path.abspath(__file__))) if __name__ == '__main__': - l = Location.Location('10.0.0.222','55556') + l = Location.Location('192.168.1.222','55556') print('starting gps') l.start_GPS() l.start_GPS_thread() print('reading data') while True: - print(l.latitude) - print(l.longitude) - print(l.bearing) + print("latitude:", l.latitude) + print("longitude:", l.longitude) + print("bearing:", l.bearing) print() sleep(1) diff --git a/examples/wheels.py b/examples/wheels.py index 711c303..8a0679f 100644 --- a/examples/wheels.py +++ b/examples/wheels.py @@ -1,3 +1,4 @@ +from time import sleep #If someone knows a better way to write the next 5 lines, lmk import os os.chdir(os.path.dirname(os.path.abspath(__file__))) @@ -7,14 +8,22 @@ from libs import UDPOut -HOST = '127.0.0.1' +HOST = '192.168.1.101' # 127.0.0.1 is the 'loopback' address, or the address # of your own computer -PORT = 2000 +PORT = 1001 # this number is arbitrary as long as it is above 1024 +speed = 90 -UDPOut.sendWheelSpeeds(HOST, PORT, 255, 255, 255, 255, 255, 255) +UDPOut.sendWheelSpeeds(HOST, PORT, speed, speed, speed, speed, speed, speed) # the six arguments represent each wheels speed (on a scale between 0 and 255, since it will be stored in a byte). # the order of the wheels in the arguments is Front Left, Middle Left, Rear Left, Front Right, Middle Right, and Rear Right. # sendWheelSpeeds(HOST, PORT, fl, ml, rl, rt, mr, rr) + +if __name__ == "__main__": + print("Sup") + speed = 90 + while True: + UDPOut.sendWheelSpeeds(HOST, PORT, speed, speed, speed, speed, speed, speed) + sleep(1) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 8ee90ae..06bf73e 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -1,286 +1,401 @@ import cv2 import cv2.aruco as aruco -import numpy as np import configparser -import sys -from time import sleep import os -''' + +""" darknetPath = os.path.dirname(os.path.abspath(__file__)) + '/../YOLO/darknet/' sys.path.append(darknetPath) from darknet_images import * from darknet import load_network -''' +""" -class ARTracker: +class ARTracker: # Constructor - #Cameras should be a list of file paths to cameras that are to be used - #set write to True to write to disk what the cameras are seeing - #set useYOLO to True to use yolo when attempting to detect the ar tags - def __init__(self, cameras, write=False, useYOLO = False, configFile="config.ini"): - self.write=write - self.distanceToMarker = -1 - self.angleToMarker = -999.9 + # Cameras should be a list of file paths to cameras that are to be used + # set write to True to write to disk what the cameras are seeing + # set use_YOLO to True to use yolo when attempting to detect the ar tags + def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini"): + self.write = write + self.distance_to_marker = -1 + self.angle_to_marker = -999.9 self.index1 = -1 self.index2 = -1 - self.useYOLO = useYOLO + self.use_YOLO = use_YOLO self.cameras = cameras - + # Open the config file config = configparser.ConfigParser(allow_no_value=True) - if not config.read(configFile): - print(f"ERROR OPENING AR CONFIG:", end="") - if os.path.isabs(configFile): - print(configFile) + if not config.read(config_file): + print("ERROR OPENING AR CONFIG:") + if os.path.isabs(config_file): + print(config_file) else: - print("{os.getcwd()}/{configFile}") + print(f"{os.getcwd()}/{config_file}") exit(-2) # Set variables from the config file - self.degreesPerPixel = float(config['ARTRACKER']['DEGREES_PER_PIXEL']) - self.vDegreesPerPixel = float(config['ARTRACKER']['VDEGREES_PER_PIXEL']) - self.focalLength = float(config['ARTRACKER']['FOCAL_LENGTH']) - self.focalLength30H = float(config['ARTRACKER']['FOCAL_LENGTH30H']) - self.focalLength30V = float(config['ARTRACKER']['FOCAL_LENGTH30V']) - self.knownMarkerWidth = float(config['ARTRACKER']['KNOWN_TAG_WIDTH']) - self.format = config['ARTRACKER']['FORMAT'] - self.frameWidth = int(config['ARTRACKER']['FRAME_WIDTH']) - self.frameHeight = int(config['ARTRACKER']['FRAME_HEIGHT']) - - #sets up yolo - if useYOLO: + self.degrees_per_pixel = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) + self.vdegrees_per_pixel = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) + self.focal_length = float(config["ARTRACKER"]["FOCAL_LENGTH"]) + self.focal_length30H = float(config["ARTRACKER"]["FOCAL_LENGTH30H"]) + self.focal_length30V = float(config["ARTRACKER"]["FOCAL_LENGTH30V"]) + self.known_marker_width = float(config["ARTRACKER"]["KNOWN_TAG_WIDTH"]) + self.format = config["ARTRACKER"]["FORMAT"] + self.frame_width = int(config["ARTRACKER"]["FRAME_WIDTH"]) + self.frame_height = int(config["ARTRACKER"]["FRAME_HEIGHT"]) + + # sets up yolo + if use_YOLO: os.chdir(darknetPath) - weights = config['YOLO']['WEIGHTS'] - cfg = config['YOLO']['CFG'] - data = config['YOLO']['DATA'] - self.thresh = float(config['YOLO']['THRESHOLD']) - self.network, self.class_names, self.class_colors = load_network(cfg, data, weights, 1) + weights = config["YOLO"]["WEIGHTS"] + cfg = config["YOLO"]["CFG"] + data = config["YOLO"]["DATA"] + self.thresh = float(config["YOLO"]["THRESHOLD"]) + self.network, self.class_names, self.class_colors = load_network( + cfg, data, weights, 1 + ) os.chdir(os.path.dirname(os.path.abspath(__file__))) - - self.networkWidth = darknet.network_width(self.network) - self.networkHeight = darknet.network_height(self.network) + + self.network_width = darknet.network_width(self.network) + self.network_height = darknet.network_height(self.network) # Initialize video writer, fps is set to 5 if self.write: - self.videoWriter = cv2.VideoWriter("autonomous.avi", cv2.VideoWriter_fourcc( - self.format[0], self.format[1], self.format[2], self.format[3]), 5, (self.frameWidth, self.frameHeight), False) - + self.video_writer = cv2.VideoWriter( + "autonomous.avi", + cv2.VideoWriter_fourcc( + self.format[0], self.format[1], self.format[2], self.format[3] + ), + 5, + (self.frame_width, self.frame_height), + False, + ) + # Set the ar marker dictionary - self.markerDict = aruco.Dictionary_get(aruco.DICT_4X4_50) - + self.marker_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) + # Initialize cameras - self.caps=[] + self.caps = [] if isinstance(self.cameras, int): self.cameras = [self.cameras] for i in range(0, len(self.cameras)): - #Makes sure the camera actually connects + # Makes sure the camera actually connects while True: cam = cv2.VideoCapture(self.cameras[i]) if not cam.isOpened(): - print(f"!!!!!!!!!!!!!!!!!!!!!!!!!!Camera ", i, " did not open!!!!!!!!!!!!!!!!!!!!!!!!!!") + print( + f"!!!!!!!!!!!!!!!!!!!!!!!!!!Camera ", + i, + " did not open!!!!!!!!!!!!!!!!!!!!!!!!!!", + ) cam.release() continue - cam.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frameHeight) - cam.set(cv2.CAP_PROP_FRAME_WIDTH, self.frameWidth) - cam.set(cv2.CAP_PROP_BUFFERSIZE, 1) # greatly speeds up the program but the writer is a bit wack because of this - cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(self.format[0], self.format[1], self.format[2], self.format[3])) - #ret, testIm = self.caps[i].read()[0]: + cam.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frame_height) + cam.set(cv2.CAP_PROP_FRAME_WIDTH, self.frame_width) + cam.set( + cv2.CAP_PROP_BUFFERSIZE, 1 + ) # greatly speeds up the program but the writer is a bit wack because of this + cam.set( + cv2.CAP_PROP_FOURCC, + cv2.VideoWriter_fourcc( + self.format[0], self.format[1], self.format[2], self.format[3] + ), + ) + # ret, testIm = self.caps[i].read()[0]: if not cam.read()[0]: cam.release() else: self.caps.append(cam) break - - #helper method to convert YOLO detections into the aruco corners format - def _convertToCorners(self,detections, numCorners): + # helper method to convert YOLO detections into the aruco corners format + # _convertToCorners + def _convert_to_corners(self, detections, num_corners): corners = [] - xCoef = self.frameWidth / self.networkWidth - yCoef = self.frameHeight / self.networkHeight - if len(detections) < numCorners: - print('ERROR, convertToCorners not used correctly') + x_coeff = self.frame_width / self.network_width + y_coeff = self.frame_height / self.network_height + if len(detections) < num_corners: + print("ERROR, convert_to_corners not used correctly") raise ValueError - for i in range(0, numCorners): - tagData = list(detections[i][2]) #Gets the x, y, width, height - - #YOLO resizes the image so this sizes it back to what we're exepcting - tagData[0] *= xCoef - tagData[1]*= yCoef - tagData[2] *= xCoef - tagData[3] *= yCoef - - #Gets the corners - topLeft = [tagData[0] - tagData[2]/2, tagData[1] - tagData[3]/2] - topRight = [tagData[0] + tagData[2]/2, tagData[1] - tagData[3]/2] - bottomRight = [tagData[0] + tagData[2]/2, tagData[1] + tagData[3]/2] - bottomLeft = [tagData[0] - tagData[2]/2, tagData[1] + tagData[3]/2] - - #appends the corners with the same format as aruco - corners.append([[topLeft, topRight, bottomRight, bottomLeft]]) - + for i in range(0, num_corners): + tag_data = list(detections[i][2]) # Gets the x, y, width, height + + # YOLO resizes the image so this sizes it back to what we're exepcting + tag_data[0] *= x_coeff + tag_data[1] *= y_coeff + tag_data[2] *= x_coeff + tag_data[3] *= y_coeff + + # Gets the corners + top_left = [tag_data[0] - tag_data[2] / 2, tag_data[1] - tag_data[3] / 2] + top_right = [tag_data[0] + tag_data[2] / 2, tag_data[1] - tag_data[3] / 2] + bottom_right = [ + tag_data[0] + tag_data[2] / 2, + tag_data[1] + tag_data[3] / 2, + ] + bottom_left = [tag_data[0] - tag_data[2] / 2, tag_data[1] + tag_data[3] / 2] + + # appends the corners with the same format as aruco + corners.append([[top_left, top_right, bottom_right, bottom_left]]) + return corners - - #id1 is the main ar tag to track, id2 is if you're looking at a gatepost, image is the image to analyze - def markerFound(self, id1, image, id2=-1): + + # id1 is the main ar tag to track, id2 is if you're looking at a gatepost, image is the image to analyze + # markerFound + def marker_found(self, id1, image, id2=-1): # converts to grayscale - cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) - + cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) + self.index1 = -1 self.index2 = -1 - bw = image #will hold the black and white image + bw = image # will hold the black and white image # tries converting to b&w using different different cutoffs to find the perfect one for the current lighting for i in range(40, 221, 60): - bw = cv2.threshold(image,i,255, cv2.THRESH_BINARY)[1] - (self.corners, self.markerIDs, self.rejected) = aruco.detectMarkers(bw, self.markerDict) - if not (self.markerIDs is None): - print('', end='') #I have not been able to reproduce an error when I have a print statement here so I'm leaving it in - if id2==-1: #single post - self.index1 = -1 + bw = cv2.threshold(image, i, 255, cv2.THRESH_BINARY)[1] + (self.corners, self.marker_IDs, self.rejected) = aruco.detectMarkers( + bw, self.marker_dict + ) + if self.marker_IDs is not None: + print( + "", end="" + ) # I have not been able to reproduce an error when I have a print statement here so I'm leaving it in + if id2 == -1: # single post + self.index1 = -1 # this just checks to make sure that it found the right marker - for m in range(len(self.markerIDs)): - if self.markerIDs[m] == id1: - self.index1 = m - break - + for m in range(len(self.marker_IDs)): + if self.marker_IDs[m] == id1: + self.index1 = m + break + if self.index1 != -1: print("Found the correct marker!") if self.write: - self.videoWriter.write(bw) #purely for debug + self.video_writer.write(bw) # purely for debug cv2.waitKey(1) - break - + break + else: - print("Found a marker but was not the correct one") - - else: #gate post + print("Found a marker but was not the correct one") + + else: # gate post self.index1 = -1 self.index2 = -1 - if len(self.markerIDs) == 1: - print('Only found marker ', self.markerIDs[0]) + if len(self.marker_IDs) == 1: + print("Only found marker ", self.marker_IDs[0]) else: - for j in range(len(self.markerIDs) - 1, -1,-1): #I trust the biggest markers the most - if self.markerIDs[j][0] == id1: - self.index1 = j - elif self.markerIDs[j][0] == id2: + for j in range( + len(self.marker_IDs) - 1, -1, -1 + ): # I trust the biggest markers the most + if self.marker_IDs[j][0] == id1: + self.index1 = j + elif self.marker_IDs[j][0] == id2: self.index2 = j if self.index1 != -1 and self.index2 != -1: - print('Found both markers!') + print("Found both markers!") if self.write: - self.videoWriter.write(bw) #purely for debug + self.video_writer.write(bw) # purely for debug cv2.waitKey(1) - break - - if i == 220: #did not find any AR markers with any b&w cutoff using aruco - #Checks to see if yolo can find a tag - if self.useYOLO: + break + + if i == 220: # did not find any AR markers with any b&w cutoff using aruco + # Checks to see if yolo can find a tag + if self.use_YOLO: detections = [] if not self.write: - #this is a simpler detection function that doesn't return the image - detections = simple_detection(image, self.network, self.class_names, self.thresh) + # this is a simpler detection function that doesn't return the image + detections = simple_detection( + image, self.network, self.class_names, self.thresh + ) else: - #more complex detection that returns the image to be written - image, detections = complex_detection(image, self.network, self.class_names, self.class_colors, self.thresh) - #cv2.imwrite('ar.jpg', image) + # more complex detection that returns the image to be written + image, detections = complex_detection( + image, + self.network, + self.class_names, + self.class_colors, + self.thresh, + ) + # cv2.imwrite('ar.jpg', image) for d in detections: print(d) - + if id2 == -1 and len(detections) > 0: - self.corners = self._convertToCorners(detections, 1) - self.index1 = 0 #Takes the highest confidence ar tag + self.corners = self._convert_to_corners(detections, 1) + self.index1 = 0 # Takes the highest confidence ar tag if self.write: - self.videoWriter.write(image) #purely for debug - cv2.waitKey(1) + self.video_writer.write(image) # purely for debug + cv2.waitKey(1) elif len(detections) > 1: - self.corners = self._convertToCorners(detections, 2) - self.index1 = 0 #takes the two highest confidence ar tags + self.corners = self._convert_to_corners(detections, 2) + self.index1 = 0 # takes the two highest confidence ar tags self.index2 = 1 if self.write: - self.videoWriter.write(image) #purely for debug + self.video_writer.write(image) # purely for debug cv2.waitKey(1) - print(self.corners) - - #Not even YOLO saw anything - if self.index1 == -1 or (self.index2 == -1 and id2 != -1): + print(self.corners) + + # Not even YOLO saw anything + if self.index1 == -1 or (self.index2 == -1 and id2 != -1): if self.write: - self.videoWriter.write(image) - #cv2.imshow('window', image) + self.video_writer.write(image) + # cv2.imshow('window', image) cv2.waitKey(1) - self.distanceToMarker = -1 - self.angleToMarker = -999 - return False - + self.distance_to_marker = -1 + self.angle_to_marker = -999 + return False + if id2 == -1: - centerXMarker = (self.corners[self.index1][0][0][0] + self.corners[self.index1][0][1][0] + \ - self.corners[self.index1][0][2][0] + self.corners[self.index1][0][3][0]) / 4 + center_x_marker = ( + self.corners[self.index1][0][0][0] + + self.corners[self.index1][0][1][0] + + self.corners[self.index1][0][2][0] + + self.corners[self.index1][0][3][0] + ) / 4 # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel - self.angleToMarker = self.degreesPerPixel * (centerXMarker - self.frameWidth/2) - - ''' + self.angle_to_marker = self.degrees_per_pixel * ( + center_x_marker - self.frame_width / 2 + ) + + """ distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker - focalLength = focal length at 0 degrees horizontal and 0 degrees vertical - focalLength30H = focal length at 30 degreees horizontal and 0 degrees vertical - focalLength30V = focal length at 30 degrees vertical and 0 degrees horizontal - realFocalLength of camera = focalLength - + (horizontal angle to marker/30) * (focalLength30H - focalLength) - + (vertical angle to marker / 30) * (focalLength30V - focalLength) - If focalLength30H and focalLength30V both equal focalLength then realFocalLength = focalLength which is good for non huddly cameras + focal_length = focal length at 0 degrees horizontal and 0 degrees vertical + focal_length30H = focal length at 30 degreees horizontal and 0 degrees vertical + focal_length30V = focal length at 30 degrees vertical and 0 degrees horizontal + realFocalLength of camera = focal_length + + (horizontal angle to marker/30) * (focal_length30H - focal_length) + + (vertical angle to marker / 30) * (focal_length30V - focal_length) + If focal_length30H and focal_length30V both equal focal_length then realFocalLength = focal_length which is good for non huddly cameras Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better - ''' - hAngleToMarker = abs(self.angleToMarker) - centerYMarker = (self.corners[self.index1][0][0][1] + self.corners[self.index1][0][1][1] + \ - self.corners[self.index1][0][2][1] + self.corners[self.index1][0][3][1]) / 4 - vAngleToMarker = abs(self.vDegreesPerPixel * (centerYMarker - self.frameHeight/2)) - realFocalLength = self.focalLength + (hAngleToMarker/30) * (self.focalLength30H - self.focalLength) + \ - (vAngleToMarker/30) * (self.focalLength30V - self.focalLength) - widthOfMarker = ((self.corners[self.index1][0][1][0] - self.corners[self.index1][0][0][0]) + \ - (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0])) / 2 - self.distanceToMarker = (self.knownMarkerWidth * realFocalLength) / widthOfMarker - + """ + hangle_to_marker = abs(self.angle_to_marker) + center_y_marker = ( + self.corners[self.index1][0][0][1] + + self.corners[self.index1][0][1][1] + + self.corners[self.index1][0][2][1] + + self.corners[self.index1][0][3][1] + ) / 4 + vangle_to_marker = abs( + self.vdegrees_per_pixel * (center_y_marker - self.frame_height / 2) + ) + realFocalLength = ( + self.focal_length + + (hangle_to_marker / 30) * (self.focal_length30H - self.focal_length) + + (vangle_to_marker / 30) * (self.focal_length30V - self.focal_length) + ) + width_of_marker = ( + ( + self.corners[self.index1][0][1][0] + - self.corners[self.index1][0][0][0] + ) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] + ) + ) / 2 + self.distance_to_marker = ( + self.known_marker_width * realFocalLength + ) / width_of_marker + else: - centerXMarker1 = (self.corners[self.index1][0][0][0] + self.corners[self.index1][0][1][0] + \ - self.corners[self.index1][0][2][0] + self.corners[self.index1][0][3][0]) / 4 - centerXMarker2 = (self.corners[self.index2][0][0][0] + self.corners[self.index2][0][1][0] + \ - self.corners[self.index2][0][2][0] + self.corners[self.index2][0][3][0]) / 4 - self.angleToMarker = self.degreesPerPixel * ((centerXMarker1 + centerXMarker2)/2 - self.frameWidth/2) - - hAngleToMarker1 = abs(self.vDegreesPerPixel * (centerXMarker1 - self.frameWidth/2)) - hAngleToMarker2 = abs(self.vDegreesPerPixel * (centerXMarker2 - self.frameWidth/2)) - centerYMarker1 = (self.corners[self.index1][0][0][1] + self.corners[self.index1][0][1][1] + \ - self.corners[self.index1][0][2][1] + self.corners[self.index1][0][3][1]) / 4 - centerYMarker2 = (self.corners[self.index2][0][0][1] + self.corners[self.index2][0][1][1] + \ - self.corners[self.index2][0][2][1] + self.corners[self.index2][0][3][1]) / 4 - vAngleToMarker1 = abs(self.vDegreesPerPixel * (centerYMarker1 - self.frameHeight/2)) - vAngleToMarker2 = abs(self.vDegreesPerPixel * (centerYMarker2 - self.frameHeight/2)) - realFocalLength1 = self.focalLength + (hAngleToMarker1/30) * (self.focalLength30H - self.focalLength) + \ - (vAngleToMarker1/30) * (self.focalLength30V - self.focalLength) - realFocalLength2 = self.focalLength + (hAngleToMarker2/30) * (self.focalLength30H - self.focalLength) + \ - (vAngleToMarker2/30) * (self.focalLength30V - self.focalLength) - widthOfMarker1 = ((self.corners[self.index1][0][1][0] - self.corners[self.index1][0][0][0]) + \ - (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0])) / 2 - widthOfMarker2 = ((self.corners[self.index2][0][1][0] - self.corners[self.index2][0][0][0]) + \ - (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0])) / 2 - - #distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker - distanceToMarker1 = (self.knownMarkerWidth * realFocalLength1) / widthOfMarker1 - distanceToMarker2 = (self.knownMarkerWidth * realFocalLength2) / widthOfMarker2 - print(f"1: {distanceToMarker1}, 2: {distanceToMarker2}") - self.distanceToMarker = (distanceToMarker1 + distanceToMarker2) / 2 - - return True - - ''' + center_x_marker1 = ( + self.corners[self.index1][0][0][0] + + self.corners[self.index1][0][1][0] + + self.corners[self.index1][0][2][0] + + self.corners[self.index1][0][3][0] + ) / 4 + center_x_marker2 = ( + self.corners[self.index2][0][0][0] + + self.corners[self.index2][0][1][0] + + self.corners[self.index2][0][2][0] + + self.corners[self.index2][0][3][0] + ) / 4 + self.angle_to_marker = self.degrees_per_pixel * ( + (center_x_marker1 + center_x_marker2) / 2 - self.frame_width / 2 + ) + + hangle_to_marker1 = abs( + self.vdegrees_per_pixel * (center_x_marker1 - self.frame_width / 2) + ) + hangle_to_marker2 = abs( + self.vdegrees_per_pixel * (center_x_marker2 - self.frame_width / 2) + ) + center_y_marker1 = ( + self.corners[self.index1][0][0][1] + + self.corners[self.index1][0][1][1] + + self.corners[self.index1][0][2][1] + + self.corners[self.index1][0][3][1] + ) / 4 + center_y_marker2 = ( + self.corners[self.index2][0][0][1] + + self.corners[self.index2][0][1][1] + + self.corners[self.index2][0][2][1] + + self.corners[self.index2][0][3][1] + ) / 4 + vangle_to_marker1 = abs( + self.vdegrees_per_pixel * (center_y_marker1 - self.frame_height / 2) + ) + vangle_to_marker2 = abs( + self.vdegrees_per_pixel * (center_y_marker2 - self.frame_height / 2) + ) + real_focal_legth1 = ( + self.focal_length + + (hangle_to_marker1 / 30) * (self.focal_length30H - self.focal_length) + + (vangle_to_marker1 / 30) * (self.focal_length30V - self.focal_length) + ) + real_focal_legth2 = ( + self.focal_length + + (hangle_to_marker2 / 30) * (self.focal_length30H - self.focal_length) + + (vangle_to_marker2 / 30) * (self.focal_length30V - self.focal_length) + ) + width_of_marker1 = ( + ( + self.corners[self.index1][0][1][0] + - self.corners[self.index1][0][0][0] + ) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] + ) + ) / 2 + width_of_marker2 = ( + ( + self.corners[self.index2][0][1][0] + - self.corners[self.index2][0][0][0] + ) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] + ) + ) / 2 + + # distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker + distance_to_marker1 = ( + self.known_marker_width * real_focal_legth1 + ) / width_of_marker1 + distance_to_marker2 = ( + self.known_marker_width * real_focal_legth2 + ) / width_of_marker2 + print(f"1: {distance_to_marker1}, 2: {distance_to_marker2}") + self.distance_to_marker = (distance_to_marker1 + distance_to_marker2) / 2 + + return True + + """ id1 is the marker you want to look for specify id2 if you want to look for a gate cameras=number of cameras to check. -1 for all of them - ''' - def findMarker(self, id1, id2=-1, cameras=-1): + """ + + # findMarker + def find_marker(self, id1, id2=-1, cameras=-1): if cameras == -1: - cameras=len(self.caps) - + cameras = len(self.caps) + for i in range(cameras): ret, frame = self.caps[i].read() - if self.markerFound(id1, frame, id2=id2): + if self.marker_found(id1, frame, id2=id2): return True return False diff --git a/libs/Drive.py b/libs/Drive.py index 13334ca..8bbf397 100644 --- a/libs/Drive.py +++ b/libs/Drive.py @@ -2,284 +2,274 @@ from threading import Timer import configparser import os +from typing import Tuple + os.chdir(os.path.dirname(os.path.abspath(__file__))) import math from time import sleep from nis import maps import sys -sys.path.append('../../Mission Control/RoverMap/') + +sys.path.append("../../Mission Control/RoverMap/") # TODO: PLEASE FIX ME from server import MapServer from libs import UDPOut from libs import Location from libs import ARTracker + class Drive: - - def __init__(self, baseSpeed, cameras): - self.baseSpeed = baseSpeed + def __init__(self, base_speed, cameras): + self.base_speed = base_speed self.tracker = ARTracker.ARTracker(cameras) - - #Starts everything needed by the map - self.mapServer = MapServer() - self.mapServer.register_routes() - self.mapServer.start(debug=False) - self.startMap(self.updateMap, .5) - sleep(.1) - - #sets up the parser + + # Starts everything needed by the map + self.map_server = MapServer() + self.map_server.register_routes() + self.map_server.start(debug=False) + self.start_map(self.update_map, 0.5) + sleep(0.1) + + # sets up the parser config = configparser.ConfigParser(allow_no_value=True) - config.read(os.path.dirname(__file__) + '/../config.ini') - - #parses config - self.mbedIP = str(config['CONFIG']['MBED_IP']) - self.mbedPort = int(config['CONFIG']['MBED_PORT']) - swiftIP = str(config['CONFIG']['SWIFT_IP']) - swiftPort = str(config['CONFIG']['SWIFT_PORT']) - self.gps = Location.Location(swiftIP, swiftPort) + config.read(os.path.dirname(__file__) + "/../config.ini") + + # parses config + self.mbed_IP = str(config["CONFIG"]["MBED_IP"]) + self.mbed_port = int(config["CONFIG"]["MBED_PORT"]) + swift_IP = str(config["CONFIG"]["SWIFT_IP"]) + swift_port = str(config["CONFIG"]["SWIFT_PORT"]) + self.gps = Location.Location(swift_IP, swift_port) self.gps.start_GPS() - - self.speeds = [0,0] - self.errorAccumulation = 0.0 - - #starts the thread that sends wheel speeds + + self.speeds = [0, 0] + self.error_accumulation = 0.0 + + # starts the thread that sends wheel speeds self.running = True - t = Thread(target=self.sendSpeed, name=('send wheel speeds'), args=()) + t = Thread(target=self.send_speed, name=("send wheel speeds"), args=()) t.daemon = True t.start() - - def startMap(self, func, seconds): + # startMap + def start_map(self, func, seconds): def func_wrapper(): - self.startMap(func, seconds) + self.start_map(func, seconds) func() + t = Timer(seconds, func_wrapper) t.start() return t - def updateMap(self): - self.mapServer.update_rover_coords([self.gps.latitude, self.gps.longitude]) - + # updateMap + def update_map(self): + self.map_server.update_rover_coords([self.gps.latitude, self.gps.longitude]) - #Every 100ms, send the current left and right wheel speeds to the mbeds - def sendSpeed(self): + # Every 100ms, send the current left and right wheel speeds to the mbeds + # sendSpeed + def send_speed(self): while self.running: + # left and right speed ls = int(self.speeds[0]) rs = int(self.speeds[1]) - UDPOut.sendWheelSpeeds(self.mbedIP, self.mbedPort, ls,ls,ls, rs,rs,rs) - sleep(.1) - - #time in milliseconds - #error in degrees - #Gets adjusted speeds based off of error and how long its been off (uses p and i) - def getSpeeds(self,speed, error, time, kp = .35, ki=.000035): - values = [0,0] - - #p and i constants if doing a pivot turn - if speed == 0: - kp = .9 - ki = .001 - - #Updates the error accumulation - self.errorAccumulation += error * time - - #Gets the adjusted speed values - values[0] = speed + (error * kp + self.errorAccumulation * ki) - values[1] = speed - (error * kp + self.errorAccumulation * ki) - - #Gets the maximum speed values depending if it is pivoting or not - min = speed - 30 - max = speed + 30 - if speed == 0: - max += 40 - min -= 40 - if max > 90: - max=90 - if min < -90: - min = -90 - - #Makes sure the adjusted speed values are within the max and mins - if values[0] > max: - values[0] = max - elif values[0] < min: - values[0] = min - if values[1] > max: - values[1] = max - elif values[1] < min: - values[1] = min - - #Makes sure the speeds are >10 or <-10. Wheels lock up if the speeds are <10 and >-10 - if values[0] <= 0 and values[0] > -10: - values[0] = -10 - elif values[0] > 0 and values[0] < 10: - values[0] = 10 - - if values[1] <= 0 and values[1] > -10: - values[1] = -10 - elif values[1] > 0 and values[1] < 10: - values[1] = 10 - - if values[0] <= 0 and values[0] > -40 and speed == 0: - values[0] = -40 - elif values[0] > 0 and values[0] < 40 and speed == 0: - values[0] = 40 - - if values[1] <= 0 and values[1] > -40 and speed == 0: - values[1] = -40 - elif values[1] > 0 and values[1] < 40 and speed == 0: - values[1] = 40 - - return values - - #Cleaner way to print out the wheel speeds - def printSpeeds(self): - print("Left wheels: ", round(self.speeds[0],1)) - print("Right wheels: ", round(self.speeds[1],1)) - - #Drives along a given list of GPS coordinates while looking for the given ar markers - #Keep id2 at -1 if looking for one post, set id1 to -1 if you aren't looking for AR markers - def driveAlongCoordinates(self, locations, id1, id2=-1): - #Starts the GPS + UDPOut.send_wheel_speeds( + self.mbed_IP, self.mbed_port, ls, ls, ls, rs, rs, rs + ) + sleep(0.1) + + # TODO: use the `pid_controller` library instead + def get_speeds(self, base_speed, error, time, kp=0.35, ki=0.000035): + """! Gets the adjusted wheel speed values using a PID controller + + @param speed The base speed for the rover + @param error The current bearing to the designated coordinate + @param time Miliseconds + @param kp P value + @param ki I value + """ + print("Current bearing to objective:", error) + + # values -> speed_values + speed_values = [0, 0] # [Left wheel speeds, right wheel speeds] + min_wheel_speed = -90 # Minimum accepted speed value + max_wheel_speed = 90 # Maximum accepted speed value + + # Updates the error accumulation + self.error_accumulation += error * time + + # Gets the adjusted speed values + speed_values[0] = base_speed + (error * kp) # + self.error_accumulation * ki) + speed_values[1] = base_speed - (error * kp) # + self.error_accumulation * ki) + + # Makes sure the adjusted speed values are within min and max wheel speed values + speed_values[0] = min(speed_values[0], max_wheel_speed) + speed_values[0] = max(speed_values[0], min_wheel_speed) + + speed_values[1] = min(speed_values[1], max_wheel_speed) + speed_values[1] = max(speed_values[1], min_wheel_speed) + + # Make sure the wheels aren't in deadlock + # Because if they're between -10 and 10 the rover won't move + if speed_values[0] < 0 and speed_values[0] > -10: + speed_values[0] = -30 + elif speed_values[0] > 0 and speed_values[0] < 10: + speed_values[0] = 30 + + if speed_values[1] < 0 and speed_values[1] > -10: + speed_values[1] = -30 + elif speed_values[1] > 0 and speed_values[1] < 10: + speed_values[1] = 30 + + return speed_values + + # Cleaner way to print out the wheel speeds + # printSpeeds + def print_speeds(self): + print("Left wheels: ", round(self.speeds[0], 1)) + print("Right wheels: ", round(self.speeds[1], 1)) + + # Drives along a given list of GPS coordinates while looking for the given ar markers + # Keep aruco_id2 at -1 if looking for one post, set aruco_id1 to -1 if you aren't looking for AR markers + # driveAlongCoordinates + def drive_along_coordinates( + self, locations: list[Tuple[float, float]], aruco_id1: int, aruco_id2: int = -1 + ) -> bool: + # Starts the GPS self.gps.start_GPS_thread() - print('Waiting for GPS connection...') - #while self.gps.all_zero: + print("Waiting for GPS connection...") + # while self.gps.all_zero: # continue - print('Connected to GPS') - - #backs up and turns to avoid running into the last detected sign. Also allows it to get a lock on heading - if(id1 > -1): - self.speeds = [-60,-60] - self.printSpeeds() + print("Connected to GPS") + + # backs up and turns to avoid running into the last detected sign. Also allows it to get a lock on heading + if aruco_id1 > -1: + self.speeds = [-60, -60] + self.print_speeds() sleep(2) - self.speeds = [0,0] - self.printSpeeds() + self.speeds = [0, 0] + self.print_speeds() sleep(2) - self.speeds = [80,20] - self.printSpeeds() + self.speeds = [80, 20] + self.print_speeds() sleep(4) else: - self.speeds = (self.baseSpeed, self.baseSpeed) - self.printSpeeds() + self.speeds = (self.base_speed, self.base_speed) + self.print_speeds() sleep(3) - #navigates to each location - for l in locations: - self.errorAccumulation = 0 - while self.gps.distance_to(l[0], l[1]) > .0025: #.0025km - bearingTo = self.gps.bearing_to(l[0], l[1]) - print(self.gps.distance_to(l[0], l[1]) ) - self.speeds = self.getSpeeds(self.baseSpeed, bearingTo, 100) #It will sleep for 100ms - sleep(.1) #Sleeps for 100ms - self.printSpeeds() - - if(id1 != -1 and self.tracker.findMarker(id1, id2)): + # navigates to each location + for lat, long in locations: + self.error_accumulation = 0 + + while self.gps.distance_to(lat, long) > 0.0025: # .0025km + bearing_to = self.gps.bearing_to(lat, long) + print(self.gps.distance_to(lat, long)) + self.speeds = self.get_speeds( + # change self.base_speed to base_speed + self.base_speed, + bearing_to, + 200, + ) # It will sleep for 100ms + sleep(0.2) # Sleeps for 100ms + self.print_speeds() + + if aruco_id1 != -1 and self.tracker.find_marker(aruco_id1, aruco_id2): self.gps.stop_GPS_thread() - print('Found Marker!') - self.speeds = [0,0] + print("Found Marker!") + self.speeds = [0, 0] return True - + self.gps.stop_GPS_thread() - print('Made it to location without seeing marker(s)') - self.speeds = [0,0] + print("Made it to location without seeing marker(s)") + self.speeds = [0, 0] return False - - def trackARMarker(self, id1, id2=-1): - stopDistance = 350 #stops when 250cm from markers TODO make sure rover doesn't stop too far away with huddlys - timesNotFound = -1 - self.tracker.findMarker(id1, id2, cameras=1) #Gets and initial angle from the main camera - self.errorAccumulation = 0 - + + def track_AR_Marker(self, aruco_id1: int, aruco_id2: int = -1): + stop_distance = 350 # stops when 250cm from markers TODO make sure rover doesn't stop too far away with huddlys + times_not_found = -1 + self.tracker.find_marker( + aruco_id1, aruco_id2, cameras=1 + ) # Gets and initial angle from the main camera + self.error_accumulation = 0 + count = 0 - #Centers the middle camera with the tag - while self.tracker.angleToMarker > 14 or self.tracker.angleToMarker < -14: - if self.tracker.findMarker(id1, id2, cameras=1): #Only looking with the center camera right now - if timesNotFound == -1: - self.speeds = [0,0] - sleep(.5) - self.speeds = [self.baseSpeed, self.baseSpeed] - sleep(.8) - self.speeds = [0,0] + # Centers the middle camera with the tag + while self.tracker.angle_to_marker > 14 or self.tracker.angle_to_marker < -14: + if self.tracker.find_marker( + aruco_id1, aruco_id2, cameras=1 + ): # Only looking with the center camera right now + if times_not_found == -1: + self.speeds = [0, 0] + sleep(0.5) + self.speeds = [self.base_speed, self.base_speed] + sleep(0.8) + self.speeds = [0, 0] else: - self.speeds = self.getSpeeds(0, self.tracker.angleToMarker, 100) - print(self.tracker.angleToMarker, " ", self.tracker.distanceToMarker) - timesNotFound = 0 - elif timesNotFound == -1: #Never seen the tag with the main camera - if(math.ceil(int(count/20)/5) % 2 == 1): - self.speeds = [self.baseSpeed+5,-self.baseSpeed-5] + self.speeds = self.get_speeds(0, self.tracker.angle_to_marker, 100) + print( + self.tracker.angle_to_marker, " ", self.tracker.distance_to_marker + ) + times_not_found = 0 + elif times_not_found == -1: # Never seen the tag with the main camera + if math.ceil(int(count / 20) / 5) % 2 == 1: + self.speeds = [self.base_speed + 5, -self.base_speed - 5] else: - self.speeds = [-self.baseSpeed-5,self.baseSpeed+5] - elif timesNotFound < 15: #Lost the tag for less than 1.5 seconds after seeing it with the main camera - timesNotFound += 1 - print(f"lost tag {timesNotFound} times") + self.speeds = [-self.base_speed - 5, self.base_speed + 5] + elif ( + times_not_found < 15 + ): # Lost the tag for less than 1.5 seconds after seeing it with the main camera + times_not_found += 1 + print(f"lost tag {times_not_found} times") else: - self.speeds = [0,0] - print("lost it") #TODO this is bad - timesNotFound = -1 - #return False - self.printSpeeds() - sleep(.1) - count+=1 - self.speeds = [0,0] - sleep(.5) - - if id2 == -1: - self.errorAccumulation = 0 - print("Locked on and ready to track") - - #Tracks down the tag - while self.tracker.distanceToMarker > stopDistance or self.tracker.distanceToMarker == -1: #-1 means we lost the tag - markerFound = self.tracker.findMarker(id1, cameras = 1) #Looks for the tag - - if self.tracker.distanceToMarker > stopDistance: - self.speeds = self.getSpeeds(self.baseSpeed-8, self.tracker.angleToMarker, 100, kp = .5, ki = .0001) - timesNotFound = 0 - print(f"Tag is {self.tracker.distanceToMarker}cm away at {self.tracker.angleToMarker} degrees") - - elif self.tracker.distanceToMarker == -1 and timesNotFound < 10: - timesNotFound += 1 - print(f"lost tag {timesNotFound} times") - - elif self.tracker.distanceToMarker == -1: - self.speeds = [0,0] - print("Lost tag") - return False #TODO this is bad - - self.printSpeeds() - sleep(.1) - - #We scored! - self.speeds = [0,0] - print("In range of the tag!") - return True - else: - #Gets the coords to the point that is 4m infront of the gate posts (get_coordinates expects distance in km) - coords = self.gps.get_coordinates(self.tracker.distanceToMarker/100000.0+.004, self.tracker.angleToMarker) - - self.speeds = [self.baseSpeed, self.baseSpeed] - sleep(5) - - #TODO: test this more after getting the new GPS - ''' - #Rover hasn't been moving for a bit so moves to get correct bearing - self.speeds = [-self.baseSpeed, -self.baseSpeed] - self.printSpeeds() - sleep(2) - self.speeds = [0,0] - self.printSpeeds() - sleep(1) - self.speeds = [self.baseSpeed, self.baseSpeed] - self.printSpeeds() - sleep(2) - - #Drives to the calculated location - while self.gps.distance_to(coords[0], coords[1]) > .003: #TODO: might want to adjust distance here... - bearingTo = self.gps.bearing_to(coords[0], coords[1]) - print(self.gps.distance_to(coords[0], coords[1]) ) - self.speeds = self.getSpeeds(self.baseSpeed, bearingTo, 100) #It will sleep for 100ms - sleep(.1) #Sleeps for 100ms - self.printSpeeds() - ''' - - - + self.speeds = [0, 0] + print("lost it") # TODO this is bad + times_not_found = -1 + # return False + self.print_speeds() + sleep(0.1) + count += 1 + self.speeds = [0, 0] + sleep(0.5) + + self.error_accumulation = 0 + print("Locked on and ready to track") + + # Tracks down the tag + while ( + self.tracker.distance_to_marker > stop_distance + or self.tracker.distance_to_marker == -1 + ): # -1 means we lost the tag + self.tracker.marker_found = self.tracker.find_marker( + aruco_id1, cameras=1 + ) # Looks for the tag + + if self.tracker.distance_to_marker > stop_distance: + self.speeds = self.get_speeds( + self.base_speed - 8, + self.tracker.angle_to_marker, + 100, + kp=0.5, + ki=0.0001, + ) + times_not_found = 0 + print( + f"Tag is {self.tracker.distance_to_marker}cm away at {self.tracker.angle_to_marker} degrees" + ) + + elif self.tracker.distance_to_marker == -1 and times_not_found < 10: + times_not_found += 1 + print(f"lost tag {times_not_found} times") + + elif self.tracker.distance_to_marker == -1: + self.speeds = [0, 0] + print("Lost tag") + return False # TODO this is bad + + self.print_speeds() + sleep(0.1) + + # We scored! + self.speeds = [0, 0] + print("In range of the tag!") + return True diff --git a/libs/Location.py b/libs/Location.py index 7d3a2cc..413876c 100644 --- a/libs/Location.py +++ b/libs/Location.py @@ -1,12 +1,15 @@ -#import sys -#sys.path.append('../') +# import sys +# sys.path.append('../') from gps import gps from math import cos, radians, degrees, sin, atan2, pi, sqrt, asin from threading import Thread from time import sleep import os + os.chdir(os.path.dirname(os.path.abspath(__file__))) + + # Class that computes functions related to location of Rover class Location: def __init__(self, ip, port): @@ -22,7 +25,7 @@ def __init__(self, ip, port): self.bearing = 0.0 self.running = True self.all_zero = True - self.wait_time = 1 + self.wait_time = 0.1 def config(self): # read from a file, probably configure this to work with @@ -30,26 +33,33 @@ def config(self): pass # Returns distance in kilometers between given latitude and longitude - def distance_to(self, lat:float, lon:float): + def distance_to(self, lat: float, lon: float): earth_radius = 6371.301 - delta_lat = (lat - self.latitude) * (pi/180.0) - delta_lon = (lon - self.longitude) * (pi/180.0) + delta_lat = (lat - self.latitude) * (pi / 180.0) + delta_lon = (lon - self.longitude) * (pi / 180.0) - a = sin(delta_lat/2) * sin(delta_lat/2) + cos(self.latitude * (pi/180.0)) * cos(lat * (pi/180.0)) * sin(delta_lon/2) * sin(delta_lon/2) - c = 2 * atan2(sqrt(a), sqrt(1-a)) + a = sin(delta_lat / 2) * sin(delta_lat / 2) + cos( + self.latitude * (pi / 180.0) + ) * cos(lat * (pi / 180.0)) * sin(delta_lon / 2) * sin(delta_lon / 2) + c = 2 * atan2(sqrt(a), sqrt(1 - a)) return earth_radius * c # Calculates difference between given bearing to location and current bearing # Positive is turn right, negative is turn left - def bearing_to(self, lat:float, lon:float): - resultbearing = self.calc_bearing(self.latitude, self.longitude, lat, lon) - self.bearing - return resultbearing + 360 if resultbearing < -180 else (resultbearing - 360 if resultbearing > 180 else resultbearing) + def bearing_to(self, lat: float, lon: float): + result_bearing = ( + self.calc_bearing(self.latitude, self.longitude, lat, lon) - self.bearing + ) + return ( + result_bearing + 360 + if result_bearing < -180 + else (result_bearing - 360 if result_bearing > 180 else result_bearing) + ) # Starts updating fields from the GPS box def start_GPS_thread(self): self.running = True - t = Thread(target=self.update_fields_loop, name=( - 'update GPS fields'), args=()) + t = Thread(target=self.update_fields_loop, name=("update GPS fields"), args=()) t.daemon = True t.start() @@ -63,11 +73,10 @@ def start_GPS(self): def stop_GPS(self): self.running = False - gps.gps_finish() - + gps.gps_finish() def update_fields_loop(self): - while(self.running): + while self.running: if gps.get_latitude() + gps.get_longitude() != 0: self.old_latitude = self.latitude self.old_longitude = self.longitude @@ -77,34 +86,42 @@ def update_fields_loop(self): self.height = gps.get_height() self.time = gps.get_time() self.error = gps.get_error() - self.bearing = self.calc_bearing(self.old_latitude, self.old_longitude, self.latitude, self.longitude) + self.bearing = self.calc_bearing( + self.old_latitude, self.old_longitude, self.latitude, self.longitude + ) self.all_zero = False else: - all_zero = True + self.all_zero = True # maybe print info or sleep or something idk sleep(self.wait_time) return - # Calculates bearing between two points. + # Calculates bearing between two points. # (0 is North, 90 is East, +/-180 is South, -90 is West) - def calc_bearing(self,lat1:float, lon1:float, lat2:float, lon2:float): - x = cos(lat2 * (pi/180.0)) * sin((lon2-lon1) * (pi/180.0)) - y = cos(lat1 * (pi/180.0)) * sin(lat2 * (pi/180.0)) - sin(lat1 * (pi/180.0)) * cos(lat2 * (pi/180.0)) * cos((lon2-lon1) * (pi/180.0)) - return (180.0/pi) * atan2(x,y) + def calc_bearing(self, lat1: float, lon1: float, lat2: float, lon2: float): + x = cos(lat2 * (pi / 180.0)) * sin((lon2 - lon1) * (pi / 180.0)) + y = cos(lat1 * (pi / 180.0)) * sin(lat2 * (pi / 180.0)) - sin( + lat1 * (pi / 180.0) + ) * cos(lat2 * (pi / 180.0)) * cos((lon2 - lon1) * (pi / 180.0)) + new_bearing = (180.0 / pi) * atan2(x, y) + # print(self.bearing - newBearing) + # if abs(self.bearing - newBearing) > 80: + # return self.bearing + return new_bearing # Calculate latitutde and longitude given distance (in km) and bearing (in degrees) - def get_coordinates(self, distance:float, bearing:float): + def get_coordinates(self, distance: float, bearing: float): # https://stackoverflow.com/questions/7222382/get-lat-long-given-current-point-distance-and-bearing R = 6371.301 - brng = radians(bearing) # Assuming bearing is in degrees + brng = radians(bearing) # Assuming bearing is in degrees d = distance - lat1 = radians(self.latitude) # Current lat point converted to radians + lat1 = radians(self.latitude) # Current lat point converted to radians lon1 = radians(self.longitude) # Current long point converted to radians - lat2 = asin(sin(lat1)*cos(d/R) + - cos(lat1)*sin(d/R)*cos(brng)) - lon2 = lon1 + atan2(sin(brng)*sin(d/R)*cos(lat1), - cos(d/R)-sin(lat1)*sin(lat2)) + lat2 = asin(sin(lat1) * cos(d / R) + cos(lat1) * sin(d / R) * cos(brng)) + lon2 = lon1 + atan2( + sin(brng) * sin(d / R) * cos(lat1), cos(d / R) - sin(lat1) * sin(lat2) + ) return degrees(lat2), degrees(lon2) diff --git a/libs/Locationf9p.py b/libs/Locationf9p.py index 5222f47..f019976 100644 --- a/libs/Locationf9p.py +++ b/libs/Locationf9p.py @@ -1,11 +1,13 @@ import sys -sys.path.append('../') + +sys.path.append("../") from math import cos, radians, degrees, sin, atan2, pi, sqrt, asin import threading + # Class that computes functions related to location of Rover # TODO: Make sure that the current GPS outputs coordinates -# in the same format as the swift, and test out +# in the same format as the swift, and test out # heading and see if that should be computed # somewhere outside of __parse class LocationF9P: @@ -28,30 +30,41 @@ def config(self): pass # Returns distance in kilometers between given latitude and longitude - def distance_to(self, lat:float, lon:float): + def distance_to(self, lat: float, lon: float): earth_radius = 6371.301 - delta_lat = (lat - self.latitude) * (pi/180.0) - delta_lon = (lon - self.longitude) * (pi/180.0) + delta_lat = (lat - self.latitude) * (pi / 180.0) + delta_lon = (lon - self.longitude) * (pi / 180.0) - a = sin(delta_lat/2) * sin(delta_lat/2) + cos(self.latitude * (pi/180.0)) * cos(lat * (pi/180.0)) * sin(delta_lon/2) * sin(delta_lon/2) - c = 2 * atan2(sqrt(a), sqrt(1-a)) + a = sin(delta_lat / 2) * sin(delta_lat / 2) + cos( + self.latitude * (pi / 180.0) + ) * cos(lat * (pi / 180.0)) * sin(delta_lon / 2) * sin(delta_lon / 2) + c = 2 * atan2(sqrt(a), sqrt(1 - a)) return earth_radius * c # Calculates difference between given bearing to location and current bearing # Positive is turn right, negative is turn left - def bearing_to(self, lat:float, lon:float): - resultbearing = self.calc_bearing(self.latitude, self.longitude, lat, lon) - self.bearing - return resultbearing + 360 if resultbearing < -180 else (resultbearing - 360 if resultbearing > 180 else resultbearing) + def bearing_to(self, lat: float, lon: float): + result_bearing = ( + self.calc_bearing(self.latitude, self.longitude, lat, lon) - self.bearing + ) + return ( + result_bearing + 360 + if result_bearing < -180 + else (result_bearing - 360 if result_bearing > 180 else result_bearing) + ) # Starts updating fields from the GPS box def start_GPS_thread(self): if self.device_open_file == None: - print("[Location-f9p.py] start_GPS_thread was called before opening the gps /dev/ entry - start_GPS will be called now") + print( + "[Location-f9p.py] start_GPS_thread was called before opening the gps /dev/ entry - start_GPS will be called now" + ) self.start_GPS() self.running = True - t = threading.Thread(target=self.update_fields_loop, name=( - 'update GPS fields'), args=()) + t = threading.Thread( + target=self.update_fields_loop, name=("update GPS fields"), args=() + ) t.daemon = True t.start() @@ -63,7 +76,7 @@ def start_GPS(self): self.device_open_file = open(self.device_path) def update_fields_loop(self): - while(self.running): + while self.running: line_read = self.device_open_file.readline() self.__parse(line_read) return @@ -120,7 +133,9 @@ def __parse(self, message_str): -1 if split[4] == "W" else 1 ) - self.bearing = self.calc_bearing(self.old_latitude, self.old_longitude, self.latitude, self.longitude) + self.bearing = self.calc_bearing( + self.old_latitude, self.old_longitude, self.latitude, self.longitude + ) elif not ( message_str == "\n" or message_str == "\r\n" or message_str == "" ) and not message_str.startswith("$"): @@ -128,26 +143,28 @@ def __parse(self, message_str): "got weird message: " + message_str + " of len " + str(len(split)) ) - # Calculates bearing between two points. + # Calculates bearing between two points. # (0 is North, 90 is East, +/-180 is South, -90 is West) - def calc_bearing(self,lat1:float, lon1:float, lat2:float, lon2:float): - x = cos(lat2 * (pi/180.0)) * sin((lon2-lon1) * (pi/180.0)) - y = cos(lat1 * (pi/180.0)) * sin(lat2 * (pi/180.0)) - sin(lat1 * (pi/180.0)) * cos(lat2 * (pi/180.0)) * cos((lon2-lon1) * (pi/180.0)) - return (180.0/pi) * atan2(x,y) + def calc_bearing(self, lat1: float, lon1: float, lat2: float, lon2: float): + x = cos(lat2 * (pi / 180.0)) * sin((lon2 - lon1) * (pi / 180.0)) + y = cos(lat1 * (pi / 180.0)) * sin(lat2 * (pi / 180.0)) - sin( + lat1 * (pi / 180.0) + ) * cos(lat2 * (pi / 180.0)) * cos((lon2 - lon1) * (pi / 180.0)) + return (180.0 / pi) * atan2(x, y) # Calculate latitutde and longitude given distance (in km) and bearing (in degrees) - def get_coordinates(self, distance:float, bearing:float): + def get_coordinates(self, distance: float, bearing: float): # https://stackoverflow.com/questions/7222382/get-lat-long-given-current-point-distance-and-bearing R = 6371.301 - brng = radians(bearing) # Assuming bearing is in degrees + brng = radians(bearing) # Assuming bearing is in degrees d = distance - lat1 = radians(self.latitude) # Current lat point converted to radians + lat1 = radians(self.latitude) # Current lat point converted to radians lon1 = radians(self.longitude) # Current long point converted to radians - lat2 = asin(sin(lat1)*cos(d/R) + - cos(lat1)*sin(d/R)*cos(brng)) - lon2 = lon1 + atan2(sin(brng)*sin(d/R)*cos(lat1), - cos(d/R)-sin(lat1)*sin(lat2)) + lat2 = asin(sin(lat1) * cos(d / R) + cos(lat1) * sin(d / R) * cos(brng)) + lon2 = lon1 + atan2( + sin(brng) * sin(d / R) * cos(lat1), cos(d / R) - sin(lat1) * sin(lat2) + ) return degrees(lat2), degrees(lon2) diff --git a/libs/UDPOut.py b/libs/UDPOut.py index 249bd07..0714497 100644 --- a/libs/UDPOut.py +++ b/libs/UDPOut.py @@ -2,62 +2,73 @@ from numpy import byte -def sendUDP(HOST,PORT,message): - #sends a message over UDP to a specific host and port + +def send_UDP(HOST, PORT, message): + # sends a message over UDP to a specific host and port BUFFERSIZE = 1024 with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: s.connect((HOST, PORT)) s.sendall(message) + # sendWheelSpeeds + -def sendWheelSpeeds(HOST, PORT, fl,ml,rl,fr,mr,rr): - #sends a udp message containing the six different wheel speeds. - #arguments correspond to front left (fl), middle left (ml), etc. +def send_wheel_speeds(HOST, PORT, fl, ml, rl, fr, mr, rr): + # sends a udp message containing the six different wheel speeds. + # arguments correspond to front left (fl), middle left (ml), etc. msg = bytearray(9) - #start byte is the # character - msg[0] = 0x23 - #byte to tell us what type of message it is. - msg[1] = 0x00 #using hexadecimal since if the code changes, it will likely have to be in hex - - #Add the speeds to the message, while simultaneously summing them for the verification bit - speeds = [fl,ml,rl,fr,mr,rr] - #print(speeds) + # start byte is the # character + msg[0] = 0x01 + # byte to tell us what type of message it is. + msg[1] = ( + 0x01 # using hexadecimal since if the code changes, it will likely have to be in hex + ) + + # Add the speeds to the message, while simultaneously summing them for the verification bit + speeds = [fl, ml, rl, fr, mr, rr] + + # Adjust speed speed_fix = [] for i in speeds: - x = (i/90.0 + 1) * 126 + x = (i / 90.0 + 1) * 126 speed_fix.append(int(x)) - #print(speed_fix) cs = 0 for i in range(6): msg[i + 2] = speed_fix[i] cs += speed_fix[i] - #add verification bit - msg[8] = cs&0xff #capped at 8 binary characters of length + # add verification bit + msg[8] = cs & 0xFF # capped at 8 binary characters of length + + # send wheel speeds + # sendUDP + send_UDP(HOST, PORT, msg) + # send_LED - #print(msg) - #send wheel speeds - sendUDP(HOST,PORT,msg) -def sendLED(HOST, PORT, color): +def send_LED(HOST, PORT, color): red = 0 green = 0 blue = 0 msg = bytearray(5) - msg[0] = 0x23 + msg[0] = 0x01 msg[1] = 0x02 - if color == 'r': + if color == "r": red = 255 - elif color == 'g': + elif color == "g": green = 255 - elif color == 'b': + elif color == "b": blue = 255 msg[2] = red msg[3] = green msg[4] = blue - sendUDP(HOST,PORT,msg) + send_UDP(HOST, PORT, msg) + if __name__ == "__main__": speed = -90 - sendWheelSpeeds('127.0.0.1', 8080, speed, speed, speed, speed, speed, speed) + send_LED( + "192.168.1.", + 1001, + ) diff --git a/main.py b/main.py index 27a710c..78fcac6 100644 --- a/main.py +++ b/main.py @@ -1,122 +1,151 @@ -import os import argparse import configparser from libs import UDPOut from libs import Drive import threading from time import sleep +from typing import Tuple +from loguru import logger -path = os.path.dirname(os.path.abspath(__file__)) -mbedIP = "10.0.0.101" -mbedPort = 1001 +def flash(mbedIP: str, mbedPort: int) -> None: + """ + Flash the LED colors green. -flashing = False - - -def flash(): - while flashing: - UDPOut.sendLED(mbedIP, mbedPort, "g") + This is used to show that the rover successfully navigated to a goal. + """ + while True: + UDPOut.send_LED(mbedIP, mbedPort, "g") sleep(0.2) - UDPOut.sendLED(mbedIP, mbedPort, "o") + UDPOut.send_LED(mbedIP, mbedPort, "o") sleep(0.2) -argParser = argparse.ArgumentParser() -argParser.add_argument( - "cameraInput", type=int, help="takes a number representing which camera to use" +# Create Argument Parser and parse arguments +arg_parser = argparse.ArgumentParser() +arg_parser.add_argument( + "--cameraid", + type=int, + help="takes an opencv number representing which camera to use", + required=True, ) -argParser.add_argument( - "-id", +arg_parser.add_argument( + "--coords", + type=str, + help="a file that has lines of `lat long` separated by spaces", + required=True, +) +arg_parser.add_argument( "--ids", type=int, - help="takes either 1 or 2 id values, defaults to -1 if id not assigned", + help="takes two aruco ids. If you are only looking for one aruco id, the second id should be -1", nargs="+", + required=True, ) -argParser.add_argument( - "-ll", - "--latLong", - type=str, - help="takes a filename for a text file, then reads that file for latlong coordinates", -) -args = argParser.parse_args() +args = arg_parser.parse_args() + + +# TODO: dataclass this function. returning a tuple is ugly and begging for bugs +def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: + """ + Parses the command line arguments. + + The returned tuple contains: + - list[int]: the ARUCO ids we'll look for + - list[Tuple[float, float]]: the GPS coordinates we'll drive to + """ + + # TODO: refactor `id_list` to use None/pattern matching + # the ARUCO tag ids that we're looking for + aruco_ids: list[int] = [-1, -1] + # coords to drive to (tuple: lat, long) + gps_coordinates: list[Tuple[float, float]] = [] + + # make sure we have two ARUCO ids + if len(aruco_ids) != 2: + raise Exception( + "Error while attempting to parse ARUCO IDs. Please enter 2 IDs to search for." + ) + # Assign Aruco IDs + aruco_ids = args.ids.copy() + + # handle the list of coordinates + with open(args.coords) as file: + for line in file: + try: + (lat_str, long_str) = line.split(" ") + + # attempt to parse the strings into two numbers + coordinates: Tuple[float, float] = ( + float(lat_str.replace("\ufeff", "")), + float(long_str.replace("\ufeff", "")), + ) + + # if we made it through, add them to the list + gps_coordinates.append(coordinates) + + except ValueError as e: + raise Exception( + f"Coordinate parsing failed on line: {line}. Error: {e}" + ) + + file.close() # important: gotta close the handle + + logger.info(f"Found the following list of coordinates: {gps_coordinates}") + return (aruco_ids, gps_coordinates) # Gets a list of coordinates from user and drives to them and then tracks the tag # Set id1 to -1 if not looking for a tag -def drive(rover): - global flashing - idList = [-1, -1] - locations = [] - - if args.ids is not None: - for i in range(len(args.ids)): - idList[i] = args.ids[i] - - id1 = idList[0] - id2 = idList[1] - - if args.latLong is not None: - with open(args.latLong) as f: - lineNum = 0 - for line in f: - lineNum += 1 - try: - coords = [ - float(item.replace("\ufeff", "")) - for item in line.strip().split() - ] - except ValueError: - print( - "Parse Error on line " - + str(lineNum) - + ": Please enter " - ) - break - else: - if len(coords) != 2: - print( - "Error on line " - + str(lineNum) - + ": Insufficient number of coordinates. Please enter " - ) - break - locations.append(coords) - f.close() - - flashing = False - UDPOut.sendLED(mbedIP, mbedPort, "r") - rover.driveAlongCoordinates(locations, id1, id2) - - if id1 != -1: - rover.trackARMarker(id1, id2) - - flashing = True - lights = threading.Thread(target=flash) - lights.start() - # UDPOut.sendLED(mbedIP, mbedPort, 'g') - - f = open("Recorded_Coordinates_" + args.latLong + ".txt", "a") - f.write("Latitude: " + rover.gps.latitude + "\n Longitude: " + rover.gps.longitude) - f.close() +# TODO: make this a part of a `Rover` class :3 +def drive( + rover: Drive, gps_coordinates: list[Tuple[float, float]], aruco_ids: list[int] +) -> bool: + """ + Given a Drive object, navigate to goal + return True if we make it the goal and False if we do not + """ -if __name__ == "__main__": - os.chdir(path) - print(os.getcwd()) - # user input (args from system) - if args.cameraInput is None: - print("ERROR: must at least specify one camera") - exit(-1) - - # gets the mbed ip and port - config = configparser.ConfigParser(allow_no_value=True) - if not config.read("config.ini"): - print("DID NOT OPEN CONFIG") - exit(-2) - mbedIP = str(config["CONFIG"]["MBED_IP"]) - mbedPort = int(config["CONFIG"]["MBED_PORT"]) + # Drive along GPS coordinates + rover.drive_along_coordinates(gps_coordinates, aruco_ids[0], aruco_ids[1]) + + # TODO: Can we have this run in the rover class instead of returning from 'drive_along_coordinates' + if aruco_ids[0] == -1: + rover.track_AR_Marker(aruco_ids[0], aruco_ids[1]) - rover = Drive.Drive(50, args.cameraInput) + return True + # TODO: Return False? If we don't make it? - drive(rover) + +if __name__ == "__main__": + # Try to read from configuration file + config: configparser = configparser.ConfigParser(allow_no_value=True) + if not config.read("config.ini"): + raise Exception( + "Failed to open configuration file! Please ensure `config.ini` exists in this directory." + ) + + # Get MBED IP and MBED Port + mbed_ip: str = str(config["CONFIG"]["MBED_IP"]) + mbed_port: int = int(config["CONFIG"]["MBED_PORT"]) + + # Read command line arguments + aruco_ids, gps_coordinates = parse_arguments() + + # Initailze REMI!! + rover: Drive = Drive.Drive(50, args.cameraInput) + + UDPOut.send_LED( + mbed_ip, mbed_port, "r" + ) # Change the LED to red to show REMI is entering Autonomous mode + + # Drive to our goal and if we succeed, flash the LEDs green + if drive(rover, gps_coordinates, aruco_ids): + logger.info("Made it goal! Flash the LEDs green!") + lights = threading.Thread(target=lambda: flash(mbed_ip, mbed_port)) + lights.start() + input("Press enter to end flashing lights") + exit() + else: + logger.info("!!!YOU FAILED!!!") From 72884f4a2b025a94a77a63354c0e4799f89090c9 Mon Sep 17 00:00:00 2001 From: Tyler Roman Date: Wed, 17 Apr 2024 18:51:30 -0500 Subject: [PATCH 02/29] Made some changes lol --- libs/ARTracker.py | 343 +++++++++++----------------------------------- 1 file changed, 82 insertions(+), 261 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 06bf73e..1a93e1a 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -3,6 +3,9 @@ import configparser import os +from loguru import logger + +# TODO: Get rid of this? """ darknetPath = os.path.dirname(os.path.abspath(__file__)) + '/../YOLO/darknet/' sys.path.append(darknetPath) @@ -10,13 +13,17 @@ from darknet import load_network """ - +# TODO(bray): `dataclasses` class ARTracker: # Constructor # Cameras should be a list of file paths to cameras that are to be used # set write to True to write to disk what the cameras are seeing # set use_YOLO to True to use yolo when attempting to detect the ar tags - def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini"): + + # TODO: Get rid of use_yolo? + # TODO: Add type declarations + # TODO: Can we not have our initialization function be over 80 lines? + def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = False, config_file: str ="config.ini") -> None: self.write = write self.distance_to_marker = -1 self.angle_to_marker = -999.9 @@ -28,7 +35,7 @@ def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini # Open the config file config = configparser.ConfigParser(allow_no_value=True) if not config.read(config_file): - print("ERROR OPENING AR CONFIG:") + logger.info("ERROR OPENING AR CONFIG:") if os.path.isabs(config_file): print(config_file) else: @@ -36,6 +43,7 @@ def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini exit(-2) # Set variables from the config file + # TODO: Could this be in a function self.degrees_per_pixel = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) self.vdegrees_per_pixel = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) self.focal_length = float(config["ARTRACKER"]["FOCAL_LENGTH"]) @@ -47,6 +55,7 @@ def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini self.frame_height = int(config["ARTRACKER"]["FRAME_HEIGHT"]) # sets up yolo + # TODO: Get rid of this? if use_YOLO: os.chdir(darknetPath) weights = config["YOLO"]["WEIGHTS"] @@ -77,6 +86,8 @@ def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini self.marker_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) # Initialize cameras + # TODO: Could this be in a function + # TODO: Also replace with logger self.caps = [] if isinstance(self.cameras, int): self.cameras = [self.cameras] @@ -85,10 +96,8 @@ def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini while True: cam = cv2.VideoCapture(self.cameras[i]) if not cam.isOpened(): - print( - f"!!!!!!!!!!!!!!!!!!!!!!!!!!Camera ", - i, - " did not open!!!!!!!!!!!!!!!!!!!!!!!!!!", + logger.info( + f"!!!!!!!!!!!!!!!!!!!!!!!!!!Camera {i} did not open!!!!!!!!!!!!!!!!!!!!!!!!!!", ) cam.release() continue @@ -103,48 +112,17 @@ def __init__(self, cameras, write=False, use_YOLO=False, config_file="config.ini self.format[0], self.format[1], self.format[2], self.format[3] ), ) - # ret, testIm = self.caps[i].read()[0]: + # ret, testIm = self.caps[i].read()[0] if not cam.read()[0]: cam.release() else: self.caps.append(cam) break - # helper method to convert YOLO detections into the aruco corners format - # _convertToCorners - def _convert_to_corners(self, detections, num_corners): - corners = [] - x_coeff = self.frame_width / self.network_width - y_coeff = self.frame_height / self.network_height - if len(detections) < num_corners: - print("ERROR, convert_to_corners not used correctly") - raise ValueError - for i in range(0, num_corners): - tag_data = list(detections[i][2]) # Gets the x, y, width, height - - # YOLO resizes the image so this sizes it back to what we're exepcting - tag_data[0] *= x_coeff - tag_data[1] *= y_coeff - tag_data[2] *= x_coeff - tag_data[3] *= y_coeff - - # Gets the corners - top_left = [tag_data[0] - tag_data[2] / 2, tag_data[1] - tag_data[3] / 2] - top_right = [tag_data[0] + tag_data[2] / 2, tag_data[1] - tag_data[3] / 2] - bottom_right = [ - tag_data[0] + tag_data[2] / 2, - tag_data[1] + tag_data[3] / 2, - ] - bottom_left = [tag_data[0] - tag_data[2] / 2, tag_data[1] + tag_data[3] / 2] - - # appends the corners with the same format as aruco - corners.append([[top_left, top_right, bottom_right, bottom_left]]) - - return corners - - # id1 is the main ar tag to track, id2 is if you're looking at a gatepost, image is the image to analyze + # id1 is the main ar tag to telating to id2, since gateposts are no longrack, id2 is not relevant, image is the image to analyze + # TODO: Get rid of anything relating to id2 # markerFound - def marker_found(self, id1, image, id2=-1): + def marker_found(self, id1: int, image, id2:int = -1) -> None: # converts to grayscale cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) @@ -158,238 +136,81 @@ def marker_found(self, id1, image, id2=-1): bw, self.marker_dict ) if self.marker_IDs is not None: - print( - "", end="" - ) # I have not been able to reproduce an error when I have a print statement here so I'm leaving it in - if id2 == -1: # single post - self.index1 = -1 - # this just checks to make sure that it found the right marker - for m in range(len(self.marker_IDs)): - if self.marker_IDs[m] == id1: - self.index1 = m - break - - if self.index1 != -1: - print("Found the correct marker!") - if self.write: - self.video_writer.write(bw) # purely for debug - cv2.waitKey(1) + # single post + self.index1 = -1 + # this just checks to make sure that it found the right marker + for m in range(len(self.marker_IDs)): + if self.marker_IDs[m] == id1: + self.index1 = m break - else: - print("Found a marker but was not the correct one") - - else: # gate post - self.index1 = -1 - self.index2 = -1 - if len(self.marker_IDs) == 1: - print("Only found marker ", self.marker_IDs[0]) - else: - for j in range( - len(self.marker_IDs) - 1, -1, -1 - ): # I trust the biggest markers the most - if self.marker_IDs[j][0] == id1: - self.index1 = j - elif self.marker_IDs[j][0] == id2: - self.index2 = j - if self.index1 != -1 and self.index2 != -1: - print("Found both markers!") - if self.write: - self.video_writer.write(bw) # purely for debug - cv2.waitKey(1) - break - - if i == 220: # did not find any AR markers with any b&w cutoff using aruco - # Checks to see if yolo can find a tag - if self.use_YOLO: - detections = [] - if not self.write: - # this is a simpler detection function that doesn't return the image - detections = simple_detection( - image, self.network, self.class_names, self.thresh - ) - else: - # more complex detection that returns the image to be written - image, detections = complex_detection( - image, - self.network, - self.class_names, - self.class_colors, - self.thresh, - ) - # cv2.imwrite('ar.jpg', image) - for d in detections: - print(d) - - if id2 == -1 and len(detections) > 0: - self.corners = self._convert_to_corners(detections, 1) - self.index1 = 0 # Takes the highest confidence ar tag - if self.write: - self.video_writer.write(image) # purely for debug - cv2.waitKey(1) - elif len(detections) > 1: - self.corners = self._convert_to_corners(detections, 2) - self.index1 = 0 # takes the two highest confidence ar tags - self.index2 = 1 - if self.write: - self.video_writer.write(image) # purely for debug - cv2.waitKey(1) - print(self.corners) - - # Not even YOLO saw anything - if self.index1 == -1 or (self.index2 == -1 and id2 != -1): + if self.index1 != -1: + logger.info("Found the correct marker!") if self.write: - self.video_writer.write(image) - # cv2.imshow('window', image) + self.video_writer.write(bw) # purely for debug cv2.waitKey(1) - self.distance_to_marker = -1 - self.angle_to_marker = -999 - return False - - if id2 == -1: - center_x_marker = ( - self.corners[self.index1][0][0][0] - + self.corners[self.index1][0][1][0] - + self.corners[self.index1][0][2][0] - + self.corners[self.index1][0][3][0] - ) / 4 - # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel - self.angle_to_marker = self.degrees_per_pixel * ( - center_x_marker - self.frame_width / 2 - ) - - """ - distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker - focal_length = focal length at 0 degrees horizontal and 0 degrees vertical - focal_length30H = focal length at 30 degreees horizontal and 0 degrees vertical - focal_length30V = focal length at 30 degrees vertical and 0 degrees horizontal - realFocalLength of camera = focal_length - + (horizontal angle to marker/30) * (focal_length30H - focal_length) - + (vertical angle to marker / 30) * (focal_length30V - focal_length) - If focal_length30H and focal_length30V both equal focal_length then realFocalLength = focal_length which is good for non huddly cameras - Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better - """ - hangle_to_marker = abs(self.angle_to_marker) - center_y_marker = ( - self.corners[self.index1][0][0][1] - + self.corners[self.index1][0][1][1] - + self.corners[self.index1][0][2][1] - + self.corners[self.index1][0][3][1] - ) / 4 - vangle_to_marker = abs( - self.vdegrees_per_pixel * (center_y_marker - self.frame_height / 2) - ) - realFocalLength = ( - self.focal_length - + (hangle_to_marker / 30) * (self.focal_length30H - self.focal_length) - + (vangle_to_marker / 30) * (self.focal_length30V - self.focal_length) - ) - width_of_marker = ( - ( - self.corners[self.index1][0][1][0] - - self.corners[self.index1][0][0][0] - ) - + ( - self.corners[self.index1][0][2][0] - - self.corners[self.index1][0][3][0] - ) - ) / 2 - self.distance_to_marker = ( - self.known_marker_width * realFocalLength - ) / width_of_marker - - else: - center_x_marker1 = ( - self.corners[self.index1][0][0][0] - + self.corners[self.index1][0][1][0] - + self.corners[self.index1][0][2][0] - + self.corners[self.index1][0][3][0] - ) / 4 - center_x_marker2 = ( - self.corners[self.index2][0][0][0] - + self.corners[self.index2][0][1][0] - + self.corners[self.index2][0][2][0] - + self.corners[self.index2][0][3][0] - ) / 4 - self.angle_to_marker = self.degrees_per_pixel * ( - (center_x_marker1 + center_x_marker2) / 2 - self.frame_width / 2 - ) - - hangle_to_marker1 = abs( - self.vdegrees_per_pixel * (center_x_marker1 - self.frame_width / 2) - ) - hangle_to_marker2 = abs( - self.vdegrees_per_pixel * (center_x_marker2 - self.frame_width / 2) - ) - center_y_marker1 = ( - self.corners[self.index1][0][0][1] - + self.corners[self.index1][0][1][1] - + self.corners[self.index1][0][2][1] - + self.corners[self.index1][0][3][1] - ) / 4 - center_y_marker2 = ( - self.corners[self.index2][0][0][1] - + self.corners[self.index2][0][1][1] - + self.corners[self.index2][0][2][1] - + self.corners[self.index2][0][3][1] - ) / 4 - vangle_to_marker1 = abs( - self.vdegrees_per_pixel * (center_y_marker1 - self.frame_height / 2) - ) - vangle_to_marker2 = abs( - self.vdegrees_per_pixel * (center_y_marker2 - self.frame_height / 2) - ) - real_focal_legth1 = ( - self.focal_length - + (hangle_to_marker1 / 30) * (self.focal_length30H - self.focal_length) - + (vangle_to_marker1 / 30) * (self.focal_length30V - self.focal_length) + break + + center_x_marker = ( + self.corners[self.index1][0][0][0] + + self.corners[self.index1][0][1][0] + + self.corners[self.index1][0][2][0] + + self.corners[self.index1][0][3][0] + ) / 4 + # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel + # FIXME(bray): wow yeah thanks for the comment, but why? + self.angle_to_marker = self.degrees_per_pixel * ( + center_x_marker - self.frame_width / 2 + ) + + """ + distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker + focal_length = focal length at 0 degrees horizontal and 0 degrees vertical + focal_length30H = focal length at 30 degreees horizontal and 0 degrees vertical + focal_length30V = focal length at 30 degrees vertical and 0 degrees horizontal + realFocalLength of camera = focal_length + + (horizontal angle to marker/30) * (focal_length30H - focal_length) + + (vertical angle to marker / 30) * (focal_length30V - focal_length) + If focal_length30H and focal_length30V both equal focal_length then realFocalLength = focal_length which is good for non huddly cameras + Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better + """ + #hangle, vangle = horizontal, vertical angle + hangle_to_marker = abs(self.angle_to_marker) + center_y_marker = ( + self.corners[self.index1][0][0][1] + + self.corners[self.index1][0][1][1] + + self.corners[self.index1][0][2][1] + + self.corners[self.index1][0][3][1] + ) / 4 + vangle_to_marker = abs( + self.vdegrees_per_pixel * (center_y_marker - self.frame_height / 2) + ) + realFocalLength = ( + self.focal_length + + (hangle_to_marker / 30) * (self.focal_length30H - self.focal_length) + + (vangle_to_marker / 30) * (self.focal_length30V - self.focal_length) + ) + width_of_marker = ( + ( + self.corners[self.index1][0][1][0] + - self.corners[self.index1][0][0][0] ) - real_focal_legth2 = ( - self.focal_length - + (hangle_to_marker2 / 30) * (self.focal_length30H - self.focal_length) - + (vangle_to_marker2 / 30) * (self.focal_length30V - self.focal_length) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] ) - width_of_marker1 = ( - ( - self.corners[self.index1][0][1][0] - - self.corners[self.index1][0][0][0] - ) - + ( - self.corners[self.index1][0][2][0] - - self.corners[self.index1][0][3][0] - ) - ) / 2 - width_of_marker2 = ( - ( - self.corners[self.index2][0][1][0] - - self.corners[self.index2][0][0][0] - ) - + ( - self.corners[self.index1][0][2][0] - - self.corners[self.index1][0][3][0] - ) - ) / 2 - - # distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker - distance_to_marker1 = ( - self.known_marker_width * real_focal_legth1 - ) / width_of_marker1 - distance_to_marker2 = ( - self.known_marker_width * real_focal_legth2 - ) / width_of_marker2 - print(f"1: {distance_to_marker1}, 2: {distance_to_marker2}") - self.distance_to_marker = (distance_to_marker1 + distance_to_marker2) / 2 + ) / 2 + self.distance_to_marker = ( + self.known_marker_width * realFocalLength + ) / width_of_marker - return True """ id1 is the marker you want to look for - specify id2 if you want to look for a gate cameras=number of cameras to check. -1 for all of them """ - # findMarker - def find_marker(self, id1, id2=-1, cameras=-1): + def find_marker(self, id1: int, id2:int = -1, cameras = -1) -> bool: if cameras == -1: cameras = len(self.caps) From 877cd8522413a8dffe63df75f490e0057e059fb2 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 17 Apr 2024 19:04:06 -0500 Subject: [PATCH 03/29] chore: Remove YOLO comment --- libs/ARTracker.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 1a93e1a..8883261 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -5,14 +5,6 @@ from loguru import logger -# TODO: Get rid of this? -""" -darknetPath = os.path.dirname(os.path.abspath(__file__)) + '/../YOLO/darknet/' -sys.path.append(darknetPath) -from darknet_images import * -from darknet import load_network -""" - # TODO(bray): `dataclasses` class ARTracker: # Constructor From a2c6eb05fe74787c335d616080bd007098a3f437 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 17 Apr 2024 19:05:39 -0500 Subject: [PATCH 04/29] fix: Missing `typing` import --- libs/ARTracker.py | 1 + 1 file changed, 1 insertion(+) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 8883261..a156be1 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -2,6 +2,7 @@ import cv2.aruco as aruco import configparser import os +from typing import List from loguru import logger From e0235448209152cdc170130f4b5ece9404b9b912 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 17 Apr 2024 19:06:44 -0500 Subject: [PATCH 05/29] chore: Remove unused YOLO init --- libs/ARTracker.py | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index a156be1..d51f7b5 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -47,22 +47,6 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal self.frame_width = int(config["ARTRACKER"]["FRAME_WIDTH"]) self.frame_height = int(config["ARTRACKER"]["FRAME_HEIGHT"]) - # sets up yolo - # TODO: Get rid of this? - if use_YOLO: - os.chdir(darknetPath) - weights = config["YOLO"]["WEIGHTS"] - cfg = config["YOLO"]["CFG"] - data = config["YOLO"]["DATA"] - self.thresh = float(config["YOLO"]["THRESHOLD"]) - self.network, self.class_names, self.class_colors = load_network( - cfg, data, weights, 1 - ) - os.chdir(os.path.dirname(os.path.abspath(__file__))) - - self.network_width = darknet.network_width(self.network) - self.network_height = darknet.network_height(self.network) - # Initialize video writer, fps is set to 5 if self.write: self.video_writer = cv2.VideoWriter( From 6583abe1d26f1ee5acf47ff021c28c593367ddf8 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 17 Apr 2024 19:24:59 -0500 Subject: [PATCH 06/29] fix: `ar.py` Example (so MyPy shuts up) --- examples/ar.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/examples/ar.py b/examples/ar.py index ac95cd1..74e1046 100644 --- a/examples/ar.py +++ b/examples/ar.py @@ -1,12 +1,10 @@ from time import sleep - -import os from libs import ARTracker -tracker = ARTracker.ARTracker(['/dev/video1'], write=False, useYOLO = False) #ARTracker requires a list of camera files +tracker = ARTracker.ARTracker([0], write=False) #ARTracker requires a list of camera files while True: - tracker.findMarker(1)#, id2 = 1) - print('Distance (in cm): ', tracker.distanceToMarker) - print('Angle: ', tracker.angleToMarker) + tracker.find_marker(1)#, id2 = 1) + print('Distance (in cm): ', tracker.distance_to_marker) + print('Angle: ', tracker.angle_to_marker) sleep(.5) From 53f9330b52b56bb510cf5f78529db19d89b68de0 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 17 Apr 2024 19:26:30 -0500 Subject: [PATCH 07/29] fix: Use modern OpenCV syntax These lines were errors. Those are from a much older version of `opencv-python` than we're targeting. --- libs/ARTracker.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index d51f7b5..04cbfa9 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -51,7 +51,7 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal if self.write: self.video_writer = cv2.VideoWriter( "autonomous.avi", - cv2.VideoWriter_fourcc( + cv2.VideoWriter.fourcc( self.format[0], self.format[1], self.format[2], self.format[3] ), 5, @@ -60,7 +60,7 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal ) # Set the ar marker dictionary - self.marker_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) + self.marker_dict = aruco.DICT_4X4_50 # Initialize cameras # TODO: Could this be in a function @@ -85,7 +85,7 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal ) # greatly speeds up the program but the writer is a bit wack because of this cam.set( cv2.CAP_PROP_FOURCC, - cv2.VideoWriter_fourcc( + cv2.VideoWriter.fourcc( self.format[0], self.format[1], self.format[2], self.format[3] ), ) @@ -109,9 +109,11 @@ def marker_found(self, id1: int, image, id2:int = -1) -> None: # tries converting to b&w using different different cutoffs to find the perfect one for the current lighting for i in range(40, 221, 60): bw = cv2.threshold(image, i, 255, cv2.THRESH_BINARY)[1] - (self.corners, self.marker_IDs, self.rejected) = aruco.detectMarkers( - bw, self.marker_dict + detector = aruco.ArucoDetector( + aruco.getPredefinedDictionary(self.marker_dict) ) + + (self.corners, self.marker_IDs, self.rejected) = detector.detectMarkers(bw) if self.marker_IDs is not None: # single post self.index1 = -1 From afa1da352a65ec205b544c8f2a2aab7ff9bb1c39 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 17 Apr 2024 19:27:25 -0500 Subject: [PATCH 08/29] fix(ARTracker.py): Return a bool in `marker_found` this needs to return a bool so our callers know what's going on :) --- libs/ARTracker.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 04cbfa9..d56ba27 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -99,7 +99,10 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal # id1 is the main ar tag to telating to id2, since gateposts are no longrack, id2 is not relevant, image is the image to analyze # TODO: Get rid of anything relating to id2 # markerFound - def marker_found(self, id1: int, image, id2:int = -1) -> None: + def marker_found(self, id1: int, image, id2: int = -1) -> bool: + + found = False + # converts to grayscale cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) @@ -125,10 +128,13 @@ def marker_found(self, id1: int, image, id2:int = -1) -> None: if self.index1 != -1: logger.info("Found the correct marker!") + found = True if self.write: self.video_writer.write(bw) # purely for debug cv2.waitKey(1) break + + return found center_x_marker = ( self.corners[self.index1][0][0][0] From c63b3f427032da2b17c34ba1e93462b142e58fcf Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 17 Apr 2024 19:28:37 -0500 Subject: [PATCH 09/29] docs(ARTracker.py): Add docstrings to some methods --- libs/ARTracker.py | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index d56ba27..4b04984 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -17,6 +17,15 @@ class ARTracker: # TODO: Add type declarations # TODO: Can we not have our initialization function be over 80 lines? def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = False, config_file: str ="config.ini") -> None: + """ + Constructs a new ARTracker. + + - `cameras` is a list of OpenCV camera values. + - `write` tells the camera to save video files. + - `config_file` is a path to the config file (relative or absolute). + """ + + self.write = write self.distance_to_marker = -1 self.angle_to_marker = -999.9 @@ -100,6 +109,11 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal # TODO: Get rid of anything relating to id2 # markerFound def marker_found(self, id1: int, image, id2: int = -1) -> bool: + """ + Attempts to find a marker with the given `id1` in the provided `image`. + + Returns true if found. + """ found = False @@ -190,12 +204,15 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: ) / width_of_marker - """ - id1 is the marker you want to look for - cameras=number of cameras to check. -1 for all of them - """ - - def find_marker(self, id1: int, id2:int = -1, cameras = -1) -> bool: + def find_marker(self, id1: int, id2: int = -1, cameras = -1) -> bool: + """ + This method attempts to find a marker with the given ID. + Returns true if found. + + `id1` is the marker you want to look for. + `cameras` is the number of cameras to check. -1 for all of them + """ + if cameras == -1: cameras = len(self.caps) From 7723b29970352e5c5bb65c94ef16d8eb2f16ff9a Mon Sep 17 00:00:00 2001 From: Barrett Date: Fri, 19 Apr 2024 16:17:29 -0500 Subject: [PATCH 10/29] docs(ARTracker.py): remove incorrect ARTracker init comments --- libs/ARTracker.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 4b04984..90d73dc 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -3,16 +3,12 @@ import configparser import os from typing import List +from cv2 import Mat from loguru import logger # TODO(bray): `dataclasses` -class ARTracker: - # Constructor - # Cameras should be a list of file paths to cameras that are to be used - # set write to True to write to disk what the cameras are seeing - # set use_YOLO to True to use yolo when attempting to detect the ar tags - +class ARTracker: # TODO: Get rid of use_yolo? # TODO: Add type declarations # TODO: Can we not have our initialization function be over 80 lines? From 32e1cdab1624850dfdf295a6d254571d61222d40 Mon Sep 17 00:00:00 2001 From: Barrett Date: Fri, 19 Apr 2024 17:18:11 -0500 Subject: [PATCH 11/29] fix(ARTracker): Early return in `marker_found` accidentally stuck that return before all those calculations. oopsie :) --- libs/ARTracker.py | 85 +++++++++++++++++++++++++---------------------- 1 file changed, 46 insertions(+), 39 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 90d73dc..65ed879 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -7,21 +7,27 @@ from loguru import logger + # TODO(bray): `dataclasses` -class ARTracker: +class ARTracker: # TODO: Get rid of use_yolo? # TODO: Add type declarations # TODO: Can we not have our initialization function be over 80 lines? - def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = False, config_file: str ="config.ini") -> None: + def __init__( + self, + cameras: List[int], + write: bool = False, + use_YOLO: bool = False, + config_file: str = "config.ini", + ) -> None: """ Constructs a new ARTracker. - + - `cameras` is a list of OpenCV camera values. - `write` tells the camera to save video files. - `config_file` is a path to the config file (relative or absolute). """ - - + self.write = write self.distance_to_marker = -1 self.angle_to_marker = -999.9 @@ -35,8 +41,9 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal if not config.read(config_file): logger.info("ERROR OPENING AR CONFIG:") if os.path.isabs(config_file): - print(config_file) + print(config_file) # print the absolute path else: + # print the full relative path print(f"{os.getcwd()}/{config_file}") exit(-2) @@ -64,12 +71,12 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal False, ) - # Set the ar marker dictionary + # Set the ARUCO marker dictionary self.marker_dict = aruco.DICT_4X4_50 # Initialize cameras # TODO: Could this be in a function - # TODO: Also replace with logger + # TODO: Also replace with logger self.caps = [] if isinstance(self.cameras, int): self.cameras = [self.cameras] @@ -78,27 +85,29 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal while True: cam = cv2.VideoCapture(self.cameras[i]) if not cam.isOpened(): - logger.info( - f"!!!!!!!!!!!!!!!!!!!!!!!!!!Camera {i} did not open!!!!!!!!!!!!!!!!!!!!!!!!!!", - ) + logger.warning(f"!!!!!!!!!!!!Camera {i} did not open!!!!!!!!!!!!!!") cam.release() continue cam.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frame_height) cam.set(cv2.CAP_PROP_FRAME_WIDTH, self.frame_width) - cam.set( - cv2.CAP_PROP_BUFFERSIZE, 1 - ) # greatly speeds up the program but the writer is a bit wack because of this + + # greatly speeds up the program but the writer is a bit wack because of this + # (it makes the camera store only one frame at a time) + cam.set(cv2.CAP_PROP_BUFFERSIZE, 1) + + # fourcc is the codec used to encode the video (generally) cam.set( cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc( self.format[0], self.format[1], self.format[2], self.format[3] ), ) - # ret, testIm = self.caps[i].read()[0] + # the 0th value of the tuple is a bool, true if we got a frame + # so, in other words: if we didn't read, stop using the camera device if not cam.read()[0]: cam.release() - else: - self.caps.append(cam) + else: # if we did get a frame... + self.caps.append(cam) # add to our list of cameras break # id1 is the main ar tag to telating to id2, since gateposts are no longrack, id2 is not relevant, image is the image to analyze @@ -107,13 +116,15 @@ def __init__(self, cameras: List[int], write: bool = False, use_YOLO: bool = Fal def marker_found(self, id1: int, image, id2: int = -1) -> bool: """ Attempts to find a marker with the given `id1` in the provided `image`. - + Returns true if found. """ - + found = False - + # converts to grayscale + # TODO(bray): we should do `grayscale = cv2.cvt_color(...)` + # otherwise, we're modifying the input image and that's fucked up cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) self.index1 = -1 @@ -121,6 +132,7 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: bw = image # will hold the black and white image # tries converting to b&w using different different cutoffs to find the perfect one for the current lighting for i in range(40, 221, 60): + # FIXME(bray): use logan's pr man! bw = cv2.threshold(image, i, 255, cv2.THRESH_BINARY)[1] detector = aruco.ArucoDetector( aruco.getPredefinedDictionary(self.marker_dict) @@ -128,7 +140,6 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: (self.corners, self.marker_IDs, self.rejected) = detector.detectMarkers(bw) if self.marker_IDs is not None: - # single post self.index1 = -1 # this just checks to make sure that it found the right marker for m in range(len(self.marker_IDs)): @@ -136,16 +147,16 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: self.index1 = m break - if self.index1 != -1: + if self.index1 != -1: # if we changed the id of the ARUCO tag... logger.info("Found the correct marker!") found = True if self.write: self.video_writer.write(bw) # purely for debug + # FIXME(bray): this is likely a bug. wait_key()'s delay + # input is in milliseconds! and 1 ms is not very long... cv2.waitKey(1) break - - return found - + center_x_marker = ( self.corners[self.index1][0][0][0] + self.corners[self.index1][0][1][0] @@ -157,7 +168,7 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: self.angle_to_marker = self.degrees_per_pixel * ( center_x_marker - self.frame_width / 2 ) - + """ distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker focal_length = focal length at 0 degrees horizontal and 0 degrees vertical @@ -169,7 +180,7 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: If focal_length30H and focal_length30V both equal focal_length then realFocalLength = focal_length which is good for non huddly cameras Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better """ - #hangle, vangle = horizontal, vertical angle + # hangle, vangle = horizontal, vertical angle hangle_to_marker = abs(self.angle_to_marker) center_y_marker = ( self.corners[self.index1][0][0][1] @@ -186,29 +197,25 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: + (vangle_to_marker / 30) * (self.focal_length30V - self.focal_length) ) width_of_marker = ( - ( - self.corners[self.index1][0][1][0] - - self.corners[self.index1][0][0][0] - ) - + ( - self.corners[self.index1][0][2][0] - - self.corners[self.index1][0][3][0] - ) + (self.corners[self.index1][0][1][0] - self.corners[self.index1][0][0][0]) + + (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0]) ) / 2 self.distance_to_marker = ( self.known_marker_width * realFocalLength ) / width_of_marker + + return found - def find_marker(self, id1: int, id2: int = -1, cameras = -1) -> bool: + def find_marker(self, id1: int, id2: int = -1, cameras: int = -1) -> bool: """ - This method attempts to find a marker with the given ID. + This method attempts to find a marker with the given ID. Returns true if found. - + `id1` is the marker you want to look for. `cameras` is the number of cameras to check. -1 for all of them """ - + if cameras == -1: cameras = len(self.caps) From 9e5f925899986db58f9c79f869323302add11168 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:40:57 -0500 Subject: [PATCH 12/29] fix(ARTracker): Remove `use_yolo` arg ...we removed the ARUCO tag YOLO like a year ago lol --- libs/ARTracker.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 65ed879..3038688 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -10,14 +10,12 @@ # TODO(bray): `dataclasses` class ARTracker: - # TODO: Get rid of use_yolo? # TODO: Add type declarations # TODO: Can we not have our initialization function be over 80 lines? def __init__( self, cameras: List[int], write: bool = False, - use_YOLO: bool = False, config_file: str = "config.ini", ) -> None: """ @@ -33,10 +31,10 @@ def __init__( self.angle_to_marker = -999.9 self.index1 = -1 self.index2 = -1 - self.use_YOLO = use_YOLO self.cameras = cameras # Open the config file + # TODO: Could this be in a function config = configparser.ConfigParser(allow_no_value=True) if not config.read(config_file): logger.info("ERROR OPENING AR CONFIG:") @@ -48,7 +46,6 @@ def __init__( exit(-2) # Set variables from the config file - # TODO: Could this be in a function self.degrees_per_pixel = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) self.vdegrees_per_pixel = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) self.focal_length = float(config["ARTRACKER"]["FOCAL_LENGTH"]) @@ -76,7 +73,6 @@ def __init__( # Initialize cameras # TODO: Could this be in a function - # TODO: Also replace with logger self.caps = [] if isinstance(self.cameras, int): self.cameras = [self.cameras] From 5555ae5f9836f30ecb7cfe851e4ee9634914b55c Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:42:12 -0500 Subject: [PATCH 13/29] fix(ARTracker): Give cap some breathing room otherwise, this can be an infinite loop (as accessing the camera can prevent the system from doing so to further initialize it) --- libs/ARTracker.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libs/ARTracker.py b/libs/ARTracker.py index 3038688..94e5699 100644 --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -3,9 +3,8 @@ import configparser import os from typing import List -from cv2 import Mat - from loguru import logger +from time import sleep # TODO(bray): `dataclasses` @@ -83,6 +82,7 @@ def __init__( if not cam.isOpened(): logger.warning(f"!!!!!!!!!!!!Camera {i} did not open!!!!!!!!!!!!!!") cam.release() + sleep(0.4) # wait a moment to try again! continue cam.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frame_height) cam.set(cv2.CAP_PROP_FRAME_WIDTH, self.frame_width) @@ -129,7 +129,7 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: # tries converting to b&w using different different cutoffs to find the perfect one for the current lighting for i in range(40, 221, 60): # FIXME(bray): use logan's pr man! - bw = cv2.threshold(image, i, 255, cv2.THRESH_BINARY)[1] + bw = cv2.threshold(grayscale, i, 255, cv2.THRESH_BINARY)[1] detector = aruco.ArucoDetector( aruco.getPredefinedDictionary(self.marker_dict) ) @@ -153,6 +153,7 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: cv2.waitKey(1) break + # TODO(teeler): Can we just replace this with cv2.arcuo.estimatePoseSingleMarkers(...) center_x_marker = ( self.corners[self.index1][0][0][0] + self.corners[self.index1][0][1][0] @@ -161,6 +162,7 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: ) / 4 # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel # FIXME(bray): wow yeah thanks for the comment, but why? + # FIXME(llar): this is making me cry, is this referencing coordinates on the aruco tag?? using x / y coordinate axis? self.angle_to_marker = self.degrees_per_pixel * ( center_x_marker - self.frame_width / 2 ) @@ -199,9 +201,8 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: self.distance_to_marker = ( self.known_marker_width * realFocalLength ) / width_of_marker - - return found + return found def find_marker(self, id1: int, id2: int = -1, cameras: int = -1) -> bool: """ From 6289dde4d6e11c49b718b360d8b76bb1b455fc95 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:42:57 -0500 Subject: [PATCH 14/29] feat(ARTracker): Copy image before grayscale --- libs/ARTracker.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) mode change 100644 => 100755 libs/ARTracker.py diff --git a/libs/ARTracker.py b/libs/ARTracker.py old mode 100644 new mode 100755 index 94e5699..f25667e --- a/libs/ARTracker.py +++ b/libs/ARTracker.py @@ -119,13 +119,11 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: found = False # converts to grayscale - # TODO(bray): we should do `grayscale = cv2.cvt_color(...)` - # otherwise, we're modifying the input image and that's fucked up - cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) + grayscale = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) self.index1 = -1 self.index2 = -1 - bw = image # will hold the black and white image + bw = grayscale # tries converting to b&w using different different cutoffs to find the perfect one for the current lighting for i in range(40, 221, 60): # FIXME(bray): use logan's pr man! From 0af648c6bf2d773bde0e13f8453be1e2a5de6738 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:43:32 -0500 Subject: [PATCH 15/29] chore: Remove unused imports --- libs/Drive.py | 5 +---- libs/Location.py | 6 ------ libs/UDPOut.py | 3 --- 3 files changed, 1 insertion(+), 13 deletions(-) mode change 100644 => 100755 libs/Location.py mode change 100644 => 100755 libs/UDPOut.py diff --git a/libs/Drive.py b/libs/Drive.py index 8bbf397..decefc3 100644 --- a/libs/Drive.py +++ b/libs/Drive.py @@ -3,14 +3,11 @@ import configparser import os from typing import Tuple - -os.chdir(os.path.dirname(os.path.abspath(__file__))) import math from time import sleep -from nis import maps - import sys +# FIXME(bray): we should be building rovermap into a package. that'd make importing easier sys.path.append("../../Mission Control/RoverMap/") # TODO: PLEASE FIX ME from server import MapServer diff --git a/libs/Location.py b/libs/Location.py old mode 100644 new mode 100755 index 413876c..0372fcf --- a/libs/Location.py +++ b/libs/Location.py @@ -1,14 +1,8 @@ -# import sys -# sys.path.append('../') from gps import gps from math import cos, radians, degrees, sin, atan2, pi, sqrt, asin from threading import Thread from time import sleep -import os - -os.chdir(os.path.dirname(os.path.abspath(__file__))) - # Class that computes functions related to location of Rover class Location: diff --git a/libs/UDPOut.py b/libs/UDPOut.py old mode 100644 new mode 100755 index 0714497..e749033 --- a/libs/UDPOut.py +++ b/libs/UDPOut.py @@ -1,11 +1,8 @@ import socket -from numpy import byte - def send_UDP(HOST, PORT, message): # sends a message over UDP to a specific host and port - BUFFERSIZE = 1024 with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: s.connect((HOST, PORT)) s.sendall(message) From c4080e5a943d51e92856eb51d4b2c1eef9b4abd4 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:45:22 -0500 Subject: [PATCH 16/29] feat(main.py): Improve config loading --- main.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/main.py b/main.py index 78fcac6..46dd764 100644 --- a/main.py +++ b/main.py @@ -1,11 +1,12 @@ import argparse -import configparser from libs import UDPOut from libs import Drive import threading from time import sleep from typing import Tuple from loguru import logger +from pathlib import Path +from configparser import ConfigParser def flash(mbedIP: str, mbedPort: int) -> None: @@ -120,10 +121,10 @@ def drive( if __name__ == "__main__": # Try to read from configuration file - config: configparser = configparser.ConfigParser(allow_no_value=True) + config: ConfigParser = ConfigParser(allow_no_value=True) if not config.read("config.ini"): raise Exception( - "Failed to open configuration file! Please ensure `config.ini` exists in this directory." + f"Failed to open configuration file! Please ensure `config.ini` exists in this directory ({Path.cwd()})." ) # Get MBED IP and MBED Port From 5285095ea9e943b40ab533e3cb5ec0582545a7c6 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:49:24 -0500 Subject: [PATCH 17/29] refactor: Use PEP 8 module names we had an error that was like: ```python3 import Drive from Drive import Drive ``` For reasons like this, it's super important to follow the spec for langs that prefer runtime checks... --- examples/ar.py | 4 ++-- examples/location.py | 22 +++++++++++----------- examples/map.py | 8 ++------ examples/wheels.py | 6 +++--- libs/{ARTracker.py => aruco_tracker.py} | 0 libs/{Drive.py => drive.py} | 14 ++++++++------ libs/{Location.py => location.py} | 0 libs/{UDPOut.py => udp_out.py} | 0 main.py | 12 ++++++------ 9 files changed, 32 insertions(+), 34 deletions(-) mode change 100644 => 100755 examples/ar.py mode change 100644 => 100755 examples/location.py mode change 100644 => 100755 examples/map.py mode change 100644 => 100755 examples/wheels.py rename libs/{ARTracker.py => aruco_tracker.py} (100%) rename libs/{Drive.py => drive.py} (96%) mode change 100644 => 100755 rename libs/{Location.py => location.py} (100%) rename libs/{UDPOut.py => udp_out.py} (100%) mode change 100644 => 100755 main.py diff --git a/examples/ar.py b/examples/ar.py old mode 100644 new mode 100755 index 74e1046..1454fff --- a/examples/ar.py +++ b/examples/ar.py @@ -1,7 +1,7 @@ from time import sleep -from libs import ARTracker +from libs import aruco_tracker -tracker = ARTracker.ARTracker([0], write=False) #ARTracker requires a list of camera files +tracker = aruco_tracker.ARTracker([0], write=False) #ARTracker requires a list of camera files while True: tracker.find_marker(1)#, id2 = 1) diff --git a/examples/location.py b/examples/location.py old mode 100644 new mode 100755 index 001c9bb..4df1cda --- a/examples/location.py +++ b/examples/location.py @@ -1,16 +1,16 @@ from time import sleep -from libs import Location +from libs.location import Location -#os.chdir(os.path.dirname(os.path.abspath(__file__))) -if __name__ == '__main__': - l = Location.Location('192.168.1.222','55556') - print('starting gps') - l.start_GPS() - l.start_GPS_thread() - print('reading data') +# os.chdir(os.path.dirname(os.path.abspath(__file__))) +if __name__ == "__main__": + locator = Location("192.168.1.222", "55556") + print("starting gps") + locator.start_GPS() + locator.start_GPS_thread() + print("reading data") while True: - print("latitude:", l.latitude) - print("longitude:", l.longitude) - print("bearing:", l.bearing) + print("latitude:", locator.latitude) + print("longitude:", locator.longitude) + print("bearing:", locator.bearing) print() sleep(1) diff --git a/examples/map.py b/examples/map.py old mode 100644 new mode 100755 index 9c4bc49..fcdc4ec --- a/examples/map.py +++ b/examples/map.py @@ -1,13 +1,9 @@ -from nis import maps import os -from random import randint, random - from scipy import rand -os.chdir(os.path.dirname(os.path.abspath(__file__))) - import threading - import sys + +os.chdir(os.path.dirname(os.path.abspath(__file__))) sys.path.append('../../Mission Control/RoverMap/') from server import MapServer diff --git a/examples/wheels.py b/examples/wheels.py old mode 100644 new mode 100755 index 8a0679f..a3fba72 --- a/examples/wheels.py +++ b/examples/wheels.py @@ -6,7 +6,7 @@ import sys sys.path.append('../') -from libs import UDPOut +from libs import udp_out HOST = '192.168.1.101' # 127.0.0.1 is the 'loopback' address, or the address @@ -16,7 +16,7 @@ # this number is arbitrary as long as it is above 1024 speed = 90 -UDPOut.sendWheelSpeeds(HOST, PORT, speed, speed, speed, speed, speed, speed) +udp_out.send_wheel_speeds(HOST, PORT, speed, speed, speed, speed, speed, speed) # the six arguments represent each wheels speed (on a scale between 0 and 255, since it will be stored in a byte). # the order of the wheels in the arguments is Front Left, Middle Left, Rear Left, Front Right, Middle Right, and Rear Right. # sendWheelSpeeds(HOST, PORT, fl, ml, rl, rt, mr, rr) @@ -25,5 +25,5 @@ print("Sup") speed = 90 while True: - UDPOut.sendWheelSpeeds(HOST, PORT, speed, speed, speed, speed, speed, speed) + udp_out.send_wheel_speeds(HOST, PORT, speed, speed, speed, speed, speed, speed) sleep(1) diff --git a/libs/ARTracker.py b/libs/aruco_tracker.py similarity index 100% rename from libs/ARTracker.py rename to libs/aruco_tracker.py diff --git a/libs/Drive.py b/libs/drive.py old mode 100644 new mode 100755 similarity index 96% rename from libs/Drive.py rename to libs/drive.py index decefc3..79618fa --- a/libs/Drive.py +++ b/libs/drive.py @@ -9,17 +9,19 @@ # FIXME(bray): we should be building rovermap into a package. that'd make importing easier sys.path.append("../../Mission Control/RoverMap/") # TODO: PLEASE FIX ME +sys.path.append("../SoonerRoverTeamVI/Mission Control/RoverMap/") +sys.path.append("../RoverMap/") from server import MapServer -from libs import UDPOut -from libs import Location -from libs import ARTracker +from libs import udp_out +from libs import location +from libs import aruco_tracker class Drive: def __init__(self, base_speed, cameras): self.base_speed = base_speed - self.tracker = ARTracker.ARTracker(cameras) + self.tracker = aruco_tracker.ARTracker(cameras) # Starts everything needed by the map self.map_server = MapServer() @@ -37,7 +39,7 @@ def __init__(self, base_speed, cameras): self.mbed_port = int(config["CONFIG"]["MBED_PORT"]) swift_IP = str(config["CONFIG"]["SWIFT_IP"]) swift_port = str(config["CONFIG"]["SWIFT_PORT"]) - self.gps = Location.Location(swift_IP, swift_port) + self.gps = location.Location(swift_IP, swift_port) self.gps.start_GPS() self.speeds = [0, 0] @@ -70,7 +72,7 @@ def send_speed(self): # left and right speed ls = int(self.speeds[0]) rs = int(self.speeds[1]) - UDPOut.send_wheel_speeds( + udp_out.send_wheel_speeds( self.mbed_IP, self.mbed_port, ls, ls, ls, rs, rs, rs ) sleep(0.1) diff --git a/libs/Location.py b/libs/location.py similarity index 100% rename from libs/Location.py rename to libs/location.py diff --git a/libs/UDPOut.py b/libs/udp_out.py similarity index 100% rename from libs/UDPOut.py rename to libs/udp_out.py diff --git a/main.py b/main.py old mode 100644 new mode 100755 index 46dd764..cec81f9 --- a/main.py +++ b/main.py @@ -1,6 +1,6 @@ import argparse -from libs import UDPOut -from libs import Drive +from libs import udp_out +from libs.drive import Drive import threading from time import sleep from typing import Tuple @@ -16,9 +16,9 @@ def flash(mbedIP: str, mbedPort: int) -> None: This is used to show that the rover successfully navigated to a goal. """ while True: - UDPOut.send_LED(mbedIP, mbedPort, "g") + udp_out.send_LED(mbedIP, mbedPort, "g") sleep(0.2) - UDPOut.send_LED(mbedIP, mbedPort, "o") + udp_out.send_LED(mbedIP, mbedPort, "o") sleep(0.2) @@ -135,9 +135,9 @@ def drive( aruco_ids, gps_coordinates = parse_arguments() # Initailze REMI!! - rover: Drive = Drive.Drive(50, args.cameraInput) + rover: Drive = Drive(50, args.cameraid) - UDPOut.send_LED( + udp_out.send_LED( mbed_ip, mbed_port, "r" ) # Change the LED to red to show REMI is entering Autonomous mode From bbf238f1c270b928afced89090f1c90adf253716 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:56:03 -0500 Subject: [PATCH 18/29] fix: Unused imports in examples --- examples/map.py | 21 +++++--------- examples/wheels.py | 11 ++----- findFocalLength.py | 71 +++++++++++++++++++++++---------------------- libs/Locationf9p.py | 7 +++-- 4 files changed, 51 insertions(+), 59 deletions(-) mode change 100644 => 100755 libs/Locationf9p.py diff --git a/examples/map.py b/examples/map.py index fcdc4ec..04bf4e5 100755 --- a/examples/map.py +++ b/examples/map.py @@ -1,18 +1,13 @@ -import os -from scipy import rand import threading import sys -os.chdir(os.path.dirname(os.path.abspath(__file__))) -sys.path.append('../../Mission Control/RoverMap/') - +sys.path.append("../../Mission Control/RoverMap/") from server import MapServer -from libs import Location - -if __name__ == '__main__': +from libs import location - loc = Location.Location('10.0.0.222', '55556') - print('Starting GPS') +if __name__ == "__main__": + loc = location.Location("10.0.0.222", "55556") + print("Starting GPS") loc.start_GPS() loc.start_GPS_thread() @@ -24,14 +19,14 @@ def set_interval(func, sec): def func_wrapper(): set_interval(func, sec) func() + t = threading.Timer(sec, func_wrapper) t.start() return t def update(): print("sending update...") - #mapServer.update_rover_coords([38.4375 + randint(0, 100) / 10000 , -110.8125]) + # mapServer.update_rover_coords([38.4375 + randint(0, 100) / 10000 , -110.8125]) mapServer.update_rover_coords([loc.latitude, loc.longitude]) - - set_interval(update, 0.500) + set_interval(update, 0.500) diff --git a/examples/wheels.py b/examples/wheels.py index a3fba72..b293495 100755 --- a/examples/wheels.py +++ b/examples/wheels.py @@ -1,14 +1,7 @@ from time import sleep -#If someone knows a better way to write the next 5 lines, lmk -import os -os.chdir(os.path.dirname(os.path.abspath(__file__))) - -import sys -sys.path.append('../') - from libs import udp_out -HOST = '192.168.1.101' +HOST = "192.168.1.101" # 127.0.0.1 is the 'loopback' address, or the address # of your own computer @@ -18,7 +11,7 @@ udp_out.send_wheel_speeds(HOST, PORT, speed, speed, speed, speed, speed, speed) # the six arguments represent each wheels speed (on a scale between 0 and 255, since it will be stored in a byte). -# the order of the wheels in the arguments is Front Left, Middle Left, Rear Left, Front Right, Middle Right, and Rear Right. +# the order of the wheels in the arguments is Front Left, Middle Left, Rear Left, Front Right, Middle Right, and Rear Right. # sendWheelSpeeds(HOST, PORT, fl, ml, rl, rt, mr, rr) if __name__ == "__main__": diff --git a/findFocalLength.py b/findFocalLength.py index d9454e5..85196f7 100755 --- a/findFocalLength.py +++ b/findFocalLength.py @@ -1,65 +1,68 @@ #!/usr/bin/python3 -#This programs returns the camera focal length assuming tag is tag 0, 20cm wide and 100cm away +# This programs returns the camera focal length assuming tag is tag 0, 20cm wide and 100cm away import cv2 import cv2.aruco as aruco -import numpy as np import os import configparser from time import sleep -#opens the camera -cam = cv2.VideoCapture('/dev/video0') +# opens the camera +cam = cv2.VideoCapture("/dev/video0") if not cam.isOpened(): print("Camera not connected") exit(-1) -#sets up the config stuff +# sets up the config stuff config = configparser.ConfigParser(allow_no_value=True) -config.read(os.path.dirname(__file__) + '/config.ini') +config.read(os.path.dirname(__file__) + "/config.ini") -#sets the resolution and 4cc -format = config['ARTRACKER']['FORMAT'] -cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(format[0], format[1], format[2], format[3])) +# sets the resolution and 4cc +format = config["ARTRACKER"]["FORMAT"] +cam.set( + cv2.CAP_PROP_FOURCC, + cv2.VideoWriter.fourcc(format[0], format[1], format[2], format[3]), +) -dppH = float(config['ARTRACKER']['DEGREES_PER_PIXEL']) -dppV = float(config['ARTRACKER']['VDEGREES_PER_PIXEL']) -width = int(config['ARTRACKER']['FRAME_WIDTH']) -height = int(config['ARTRACKER']['FRAME_HEIGHT']) +dppH = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) +dppV = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) +width = int(config["ARTRACKER"]["FRAME_WIDTH"]) +height = int(config["ARTRACKER"]["FRAME_HEIGHT"]) cam.set(cv2.CAP_PROP_FRAME_WIDTH, width) cam.set(cv2.CAP_PROP_FRAME_HEIGHT, height) -tagDict = aruco.Dictionary_get(aruco.DICT_4X4_50) +tagDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) while True: try: - #takes image and detects markers + # takes image and detects markers ret, image = cam.read() - corners, markerIDs, rejected = aruco.detectMarkers(image, tagDict) - + detector = aruco.ArucoDetector(tagDict) + (corners, markerIDs, rejected) = detector.detectMarkers(image) + tagWidth = 0 - if not markerIDs is None and markerIDs[0] == 4: + if markerIDs is not None and markerIDs[0] == 4: tagWidth = corners[0][0][1][0] - corners[0][0][0][0] - - #IMPORTANT: Assumes tag is 20cm wide and 100cm away + + # IMPORTANT: Assumes tag is 20cm wide and 100cm away focalLength = (tagWidth * 100) / 20 print("Focal length: ", focalLength) - centerXMarker = (corners[0][0][1][0] + corners[0][0][0][0]) / 2 - angleToMarkerH = dppH * (centerXMarker - width/2) - print('Horizontal angle: ', angleToMarkerH) - - centerYMarker = (corners[0][0][0][1] + corners[0][0][2][1]) / 2 - angleToMarkerV = -1 *dppV * (centerYMarker - height/2) - print('Vertical angle: ', angleToMarkerV) + centerXMarker = (corners[0][0][1][0] + corners[0][0][0][0]) / 2 + angleToMarkerH = dppH * (centerXMarker - width / 2) + print("Horizontal angle: ", angleToMarkerH) + + centerYMarker = (corners[0][0][0][1] + corners[0][0][2][1]) / 2 + angleToMarkerV = -1 * dppV * (centerYMarker - height / 2) + print("Vertical angle: ", angleToMarkerV) else: print("Nothing found") - - sleep(.1) - + + sleep(0.1) + except KeyboardInterrupt: break - except: - pass #When running multiple times in a row, must cycle through a few times - -cam.release() #releases the camera + except Exception: + pass # When running multiple times in a row, must cycle through a few times + +cam.release() # releases the camera diff --git a/libs/Locationf9p.py b/libs/Locationf9p.py old mode 100644 new mode 100755 index f019976..a32ba25 --- a/libs/Locationf9p.py +++ b/libs/Locationf9p.py @@ -55,7 +55,7 @@ def bearing_to(self, lat: float, lon: float): # Starts updating fields from the GPS box def start_GPS_thread(self): - if self.device_open_file == None: + if self.device_open_file is None: print( "[Location-f9p.py] start_GPS_thread was called before opening the gps /dev/ entry - start_GPS will be called now" ) @@ -77,8 +77,9 @@ def start_GPS(self): def update_fields_loop(self): while self.running: - line_read = self.device_open_file.readline() - self.__parse(line_read) + if self.device_open_file is not None: + line_read = self.device_open_file.readline() + self.__parse(line_read) return # Parse NMEA messages read from the GPS. From 770f3352921842c678da4f72546b4fd02bd48325 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 20 Apr 2024 13:58:19 -0500 Subject: [PATCH 19/29] fix: Correct "goal found" grammar --- main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main.py b/main.py index cec81f9..96257a3 100755 --- a/main.py +++ b/main.py @@ -143,7 +143,7 @@ def drive( # Drive to our goal and if we succeed, flash the LEDs green if drive(rover, gps_coordinates, aruco_ids): - logger.info("Made it goal! Flash the LEDs green!") + logger.info("Made it to the goal! Flash the LEDs green!") lights = threading.Thread(target=lambda: flash(mbed_ip, mbed_port)) lights.start() input("Press enter to end flashing lights") From ea6d3e35198ddd0dc3efd4208c04a77ee667ce70 Mon Sep 17 00:00:00 2001 From: Barrett Date: Thu, 16 May 2024 16:30:11 -0500 Subject: [PATCH 20/29] feat: Use Rye --- .python-version | 1 + pyproject.toml | 25 +++++++++++++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 .python-version create mode 100644 pyproject.toml diff --git a/.python-version b/.python-version new file mode 100644 index 0000000..8531a3b --- /dev/null +++ b/.python-version @@ -0,0 +1 @@ +3.12.2 diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..b9e0148 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,25 @@ +[project] +name = "autonomous" +version = "0.1.0" +description = "Add your description here" +authors = [{ name = "Barrett", email = "contact@barretts.club" }] +dependencies = [ + "opencv-python>=4.7", + "flask>=3.0.3", + "loguru>=0.7.2", +] +readme = "README.md" +requires-python = ">= 3.10" +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[tool.rye] +managed = true +dev-dependencies = ["mypy>=1.8.0", "ruff>=0.2.0"] + +[tool.hatch.metadata] +allow-direct-references = true + +[tool.hatch.build.targets.wheel] +packages = ["./"] From 5ce09cc67036519ee6b552302a4b8c2f5aff121d Mon Sep 17 00:00:00 2001 From: Barrett Date: Thu, 16 May 2024 18:02:37 -0500 Subject: [PATCH 21/29] fix(examples/ar): Handle case where marker isn't found --- examples/ar.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/examples/ar.py b/examples/ar.py index 1454fff..44cca87 100755 --- a/examples/ar.py +++ b/examples/ar.py @@ -1,10 +1,16 @@ from time import sleep from libs import aruco_tracker +from loguru import logger -tracker = aruco_tracker.ARTracker([0], write=False) #ARTracker requires a list of camera files +tracker = aruco_tracker.ARTracker([0], write=False) # ARTracker requires a list of camera IDs +marker = 1 # change this ARUCO identifier if you need to :) while True: - tracker.find_marker(1)#, id2 = 1) - print('Distance (in cm): ', tracker.distance_to_marker) - print('Angle: ', tracker.angle_to_marker) - sleep(.5) + found = tracker.find_marker(marker) + + if found: + print('Distance (in cm): ', tracker.distance_to_marker) + print('Angle: ', tracker.angle_to_marker) + else: + logger.error(f"Didn't find marker {marker}") + sleep(0.5) From 30f3b6c16b262383c5c134a9cb2a12607bfe1895 Mon Sep 17 00:00:00 2001 From: Barrett Date: Thu, 16 May 2024 18:04:22 -0500 Subject: [PATCH 22/29] fix(aruco_tracker.py): Check for detection failure --- libs/aruco_tracker.py | 102 +++++++++++++++++++++++------------------- 1 file changed, 55 insertions(+), 47 deletions(-) diff --git a/libs/aruco_tracker.py b/libs/aruco_tracker.py index f25667e..74b4c61 100755 --- a/libs/aruco_tracker.py +++ b/libs/aruco_tracker.py @@ -151,54 +151,62 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: cv2.waitKey(1) break - # TODO(teeler): Can we just replace this with cv2.arcuo.estimatePoseSingleMarkers(...) - center_x_marker = ( - self.corners[self.index1][0][0][0] - + self.corners[self.index1][0][1][0] - + self.corners[self.index1][0][2][0] - + self.corners[self.index1][0][3][0] - ) / 4 - # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel - # FIXME(bray): wow yeah thanks for the comment, but why? - # FIXME(llar): this is making me cry, is this referencing coordinates on the aruco tag?? using x / y coordinate axis? - self.angle_to_marker = self.degrees_per_pixel * ( - center_x_marker - self.frame_width / 2 - ) + # we need to check if we've found it or not. so... + if self.index1 != -1: + # TODO(teeler): Can we just replace this with cv2.arcuo.estimatePoseSingleMarkers(...) + center_x_marker = ( + self.corners[self.index1][0][0][0] + + self.corners[self.index1][0][1][0] + + self.corners[self.index1][0][2][0] + + self.corners[self.index1][0][3][0] + ) / 4 + # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel + # FIXME(bray): wow yeah thanks for the comment, but why? + # FIXME(llar): this is making me cry, is this referencing coordinates on the aruco tag?? using x / y coordinate axis? + self.angle_to_marker = self.degrees_per_pixel * ( + center_x_marker - self.frame_width / 2 + ) - """ - distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker - focal_length = focal length at 0 degrees horizontal and 0 degrees vertical - focal_length30H = focal length at 30 degreees horizontal and 0 degrees vertical - focal_length30V = focal length at 30 degrees vertical and 0 degrees horizontal - realFocalLength of camera = focal_length - + (horizontal angle to marker/30) * (focal_length30H - focal_length) - + (vertical angle to marker / 30) * (focal_length30V - focal_length) - If focal_length30H and focal_length30V both equal focal_length then realFocalLength = focal_length which is good for non huddly cameras - Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better - """ - # hangle, vangle = horizontal, vertical angle - hangle_to_marker = abs(self.angle_to_marker) - center_y_marker = ( - self.corners[self.index1][0][0][1] - + self.corners[self.index1][0][1][1] - + self.corners[self.index1][0][2][1] - + self.corners[self.index1][0][3][1] - ) / 4 - vangle_to_marker = abs( - self.vdegrees_per_pixel * (center_y_marker - self.frame_height / 2) - ) - realFocalLength = ( - self.focal_length - + (hangle_to_marker / 30) * (self.focal_length30H - self.focal_length) - + (vangle_to_marker / 30) * (self.focal_length30V - self.focal_length) - ) - width_of_marker = ( - (self.corners[self.index1][0][1][0] - self.corners[self.index1][0][0][0]) - + (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0]) - ) / 2 - self.distance_to_marker = ( - self.known_marker_width * realFocalLength - ) / width_of_marker + """ + distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker + focal_length = focal length at 0 degrees horizontal and 0 degrees vertical + focal_length30H = focal length at 30 degreees horizontal and 0 degrees vertical + focal_length30V = focal length at 30 degrees vertical and 0 degrees horizontal + realFocalLength of camera = focal_length + + (horizontal angle to marker/30) * (focal_length30H - focal_length) + + (vertical angle to marker / 30) * (focal_length30V - focal_length) + If focal_length30H and focal_length30V both equal focal_length then realFocalLength = focal_length which is good for non huddly cameras + Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better + """ + # hangle, vangle = horizontal, vertical angle + hangle_to_marker = abs(self.angle_to_marker) + center_y_marker = ( + self.corners[self.index1][0][0][1] + + self.corners[self.index1][0][1][1] + + self.corners[self.index1][0][2][1] + + self.corners[self.index1][0][3][1] + ) / 4 + vangle_to_marker = abs( + self.vdegrees_per_pixel * (center_y_marker - self.frame_height / 2) + ) + realFocalLength = ( + self.focal_length + + (hangle_to_marker / 30) * (self.focal_length30H - self.focal_length) + + (vangle_to_marker / 30) * (self.focal_length30V - self.focal_length) + ) + width_of_marker = ( + ( + self.corners[self.index1][0][1][0] + - self.corners[self.index1][0][0][0] + ) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] + ) + ) / 2 + self.distance_to_marker = ( + self.known_marker_width * realFocalLength + ) / width_of_marker return found From 5c1e37e3544eeb80dbf5bef5aad0f4a3d4e4fbd4 Mon Sep 17 00:00:00 2001 From: Barrett Date: Tue, 21 May 2024 15:04:49 -0500 Subject: [PATCH 23/29] fix(ar_tracker): use Python3.6 (old cv2) syntax --- libs/aruco_tracker.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/libs/aruco_tracker.py b/libs/aruco_tracker.py index 74b4c61..2e18ace 100755 --- a/libs/aruco_tracker.py +++ b/libs/aruco_tracker.py @@ -1,10 +1,11 @@ -import cv2 -import cv2.aruco as aruco import configparser import os +from time import sleep from typing import List + +import cv2 +import cv2.aruco as aruco from loguru import logger -from time import sleep # TODO(bray): `dataclasses` @@ -59,7 +60,7 @@ def __init__( if self.write: self.video_writer = cv2.VideoWriter( "autonomous.avi", - cv2.VideoWriter.fourcc( + cv2.VideoWriter_fourcc( self.format[0], self.format[1], self.format[2], self.format[3] ), 5, @@ -68,7 +69,7 @@ def __init__( ) # Set the ARUCO marker dictionary - self.marker_dict = aruco.DICT_4X4_50 + self.marker_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) # Initialize cameras # TODO: Could this be in a function @@ -94,7 +95,7 @@ def __init__( # fourcc is the codec used to encode the video (generally) cam.set( cv2.CAP_PROP_FOURCC, - cv2.VideoWriter.fourcc( + cv2.VideoWriter_fourcc( self.format[0], self.format[1], self.format[2], self.format[3] ), ) @@ -128,11 +129,9 @@ def marker_found(self, id1: int, image, id2: int = -1) -> bool: for i in range(40, 221, 60): # FIXME(bray): use logan's pr man! bw = cv2.threshold(grayscale, i, 255, cv2.THRESH_BINARY)[1] - detector = aruco.ArucoDetector( - aruco.getPredefinedDictionary(self.marker_dict) + (self.corners, self.marker_IDs, self.rejected) = aruco.detectMarkers( + bw, self.marker_dict ) - - (self.corners, self.marker_IDs, self.rejected) = detector.detectMarkers(bw) if self.marker_IDs is not None: self.index1 = -1 # this just checks to make sure that it found the right marker From d4417810d38c7accd9c2ea2a69f8a3d1811fe150 Mon Sep 17 00:00:00 2001 From: REMI Date: Wed, 22 May 2024 12:45:52 -0500 Subject: [PATCH 24/29] fix(main): may 21st debugging (pre-refactor) --- main.py | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/main.py b/main.py index 96257a3..df128c9 100755 --- a/main.py +++ b/main.py @@ -15,6 +15,7 @@ def flash(mbedIP: str, mbedPort: int) -> None: This is used to show that the rover successfully navigated to a goal. """ + while True: udp_out.send_LED(mbedIP, mbedPort, "g") sleep(0.2) @@ -22,6 +23,7 @@ def flash(mbedIP: str, mbedPort: int) -> None: sleep(0.2) + # Create Argument Parser and parse arguments arg_parser = argparse.ArgumentParser() arg_parser.add_argument( @@ -47,7 +49,9 @@ def flash(mbedIP: str, mbedPort: int) -> None: # TODO: dataclass this function. returning a tuple is ugly and begging for bugs -def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: +# TODO: use typing. broken due to old ass python (<3.8) +#def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: +def parse_arguments(): """ Parses the command line arguments. @@ -58,7 +62,7 @@ def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: # TODO: refactor `id_list` to use None/pattern matching # the ARUCO tag ids that we're looking for - aruco_ids: list[int] = [-1, -1] + aruco_ids: list[int] = args.ids.copy() # coords to drive to (tuple: lat, long) gps_coordinates: list[Tuple[float, float]] = [] @@ -67,8 +71,6 @@ def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: raise Exception( "Error while attempting to parse ARUCO IDs. Please enter 2 IDs to search for." ) - # Assign Aruco IDs - aruco_ids = args.ids.copy() # handle the list of coordinates with open(args.coords) as file: @@ -100,7 +102,9 @@ def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: # Set id1 to -1 if not looking for a tag # TODO: make this a part of a `Rover` class :3 def drive( - rover: Drive, gps_coordinates: list[Tuple[float, float]], aruco_ids: list[int] + # TODO: use typing. old ass python (<3.8) + #rover: Drive, gps_coordinates: list[Tuple[float, float]], aruco_ids: list[int] + rover: Drive, gps_coordinates, aruco_ids ) -> bool: """ Given a Drive object, navigate to goal @@ -109,10 +113,13 @@ def drive( """ # Drive along GPS coordinates - rover.drive_along_coordinates(gps_coordinates, aruco_ids[0], aruco_ids[1]) + logger.warning("ok we're driving along the gps coords :3") + dac_result = rover.drive_along_coordinates(gps_coordinates, aruco_ids[0], aruco_ids[1]) + logger.error(f"drive along coordinates returned {dac_result}") + # TODO: Can we have this run in the rover class instead of returning from 'drive_along_coordinates' - if aruco_ids[0] == -1: + if aruco_ids[0] != -1: rover.track_AR_Marker(aruco_ids[0], aruco_ids[1]) return True @@ -132,11 +139,14 @@ def drive( mbed_port: int = int(config["CONFIG"]["MBED_PORT"]) # Read command line arguments + logger.info("parsing command line arugments") aruco_ids, gps_coordinates = parse_arguments() # Initailze REMI!! + logger.info("creating a Drive object (init remi)") rover: Drive = Drive(50, args.cameraid) + logger.info("changing led to indicate autonomous mode") udp_out.send_LED( mbed_ip, mbed_port, "r" ) # Change the LED to red to show REMI is entering Autonomous mode @@ -146,7 +156,8 @@ def drive( logger.info("Made it to the goal! Flash the LEDs green!") lights = threading.Thread(target=lambda: flash(mbed_ip, mbed_port)) lights.start() - input("Press enter to end flashing lights") + n = input("Press enter to end flashing lights") + logger.info("Exiting Program") exit() else: logger.info("!!!YOU FAILED!!!") From 04c1a1f3f340dab430a2f22f731c988f3693c1c0 Mon Sep 17 00:00:00 2001 From: REMI Date: Wed, 22 May 2024 12:46:28 -0500 Subject: [PATCH 25/29] chore: Default to Python 3.8 --- .python-version | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.python-version b/.python-version index 8531a3b..9ad6380 100644 --- a/.python-version +++ b/.python-version @@ -1 +1 @@ -3.12.2 +3.8.18 From 539cce9ef607f3020e183baa03af02e8f426bd66 Mon Sep 17 00:00:00 2001 From: REMI Date: Wed, 22 May 2024 12:47:57 -0500 Subject: [PATCH 26/29] feat(drive.py): use logger for prints --- libs/drive.py | 68 +++++++++++++++++++++++++++++---------------------- 1 file changed, 39 insertions(+), 29 deletions(-) diff --git a/libs/drive.py b/libs/drive.py index 79618fa..5cf5c1e 100755 --- a/libs/drive.py +++ b/libs/drive.py @@ -6,12 +6,10 @@ import math from time import sleep import sys +from loguru import logger # FIXME(bray): we should be building rovermap into a package. that'd make importing easier -sys.path.append("../../Mission Control/RoverMap/") # TODO: PLEASE FIX ME -sys.path.append("../SoonerRoverTeamVI/Mission Control/RoverMap/") -sys.path.append("../RoverMap/") -from server import MapServer +from maps.server import MapServer from libs import udp_out from libs import location @@ -87,7 +85,7 @@ def get_speeds(self, base_speed, error, time, kp=0.35, ki=0.000035): @param kp P value @param ki I value """ - print("Current bearing to objective:", error) + logger.info("Current bearing to objective:", error) # values -> speed_values speed_values = [0, 0] # [Left wheel speeds, right wheel speeds] @@ -121,66 +119,75 @@ def get_speeds(self, base_speed, error, time, kp=0.35, ki=0.000035): speed_values[1] = 30 return speed_values + + #def left_wheel_speed(self) -> int: # Cleaner way to print out the wheel speeds # printSpeeds def print_speeds(self): - print("Left wheels: ", round(self.speeds[0], 1)) - print("Right wheels: ", round(self.speeds[1], 1)) + logger.info(f"Left wheel speed: {self.speeds[0]}") + logger.info(f"Right: wheel speed: {self.speeds[1]}") # Drives along a given list of GPS coordinates while looking for the given ar markers # Keep aruco_id2 at -1 if looking for one post, set aruco_id1 to -1 if you aren't looking for AR markers # driveAlongCoordinates def drive_along_coordinates( - self, locations: list[Tuple[float, float]], aruco_id1: int, aruco_id2: int = -1 + # TODO: use below. doesn't work on old ass python (<3.8) + # self, locations: list[Tuple[float, float]], aruco_id1: int, aruco_id2: int = 1 + self, locations, aruco_id1: int, aruco_id2: int = -1 ) -> bool: # Starts the GPS + logger.info("starting gps thread...") self.gps.start_GPS_thread() - print("Waiting for GPS connection...") - # while self.gps.all_zero: - # continue - print("Connected to GPS") + logger.info("gps is up!") # backs up and turns to avoid running into the last detected sign. Also allows it to get a lock on heading if aruco_id1 > -1: - self.speeds = [-60, -60] + logger.info("avoiding last detected marker...") + self.speeds = [-60, -60] # reverse both sides self.print_speeds() sleep(2) - self.speeds = [0, 0] + self.speeds = [0, 0] # stop for a moment self.print_speeds() sleep(2) - self.speeds = [80, 20] + self.speeds = [80, 20] # turn right self.print_speeds() sleep(4) else: - self.speeds = (self.base_speed, self.base_speed) + # otherwise, we drive forward + logger.info("no previous marker detected. let's drive forward!") + self.speeds = [self.base_speed, self.base_speed] self.print_speeds() sleep(3) # navigates to each location + logger.info("navigating to each location...") for lat, long in locations: + distance_checks: int = 0 self.error_accumulation = 0 while self.gps.distance_to(lat, long) > 0.0025: # .0025km + distance_checks += 1 bearing_to = self.gps.bearing_to(lat, long) - print(self.gps.distance_to(lat, long)) + logger.info(f"distance to location ({lat}, {long}): {self.gps.distance_to(lat, long)}") self.speeds = self.get_speeds( # change self.base_speed to base_speed self.base_speed, bearing_to, 200, ) # It will sleep for 100ms - sleep(0.2) # Sleeps for 100ms + sleep(0.1) # Sleeps for 100ms self.print_speeds() + logger.info(f"distance checked {distance_checks} times") if aruco_id1 != -1 and self.tracker.find_marker(aruco_id1, aruco_id2): self.gps.stop_GPS_thread() - print("Found Marker!") + logger.info("Found Marker!") self.speeds = [0, 0] return True self.gps.stop_GPS_thread() - print("Made it to location without seeing marker(s)") + logger.warning("Made it to location without seeing marker(s)") self.speeds = [0, 0] return False @@ -193,24 +200,27 @@ def track_AR_Marker(self, aruco_id1: int, aruco_id2: int = -1): self.error_accumulation = 0 count = 0 - # Centers the middle camera with the tag + # Pivot the rover until it is in line with the aruco tag while self.tracker.angle_to_marker > 14 or self.tracker.angle_to_marker < -14: + # If the tag is in the cameras fov, move towards the aruco tag if self.tracker.find_marker( aruco_id1, aruco_id2, cameras=1 ): # Only looking with the center camera right now - if times_not_found == -1: + if times_not_found == -1: # FIXME: Why are we checking if it's not found if we see it in the camera's fov self.speeds = [0, 0] sleep(0.5) self.speeds = [self.base_speed, self.base_speed] sleep(0.8) self.speeds = [0, 0] - else: + else: # Move towards the rover self.speeds = self.get_speeds(0, self.tracker.angle_to_marker, 100) - print( - self.tracker.angle_to_marker, " ", self.tracker.distance_to_marker + logger.info( + f"{self.tracker.angle_to_marker} {self.tracker.distance_to_marker}" ) times_not_found = 0 - elif times_not_found == -1: # Never seen the tag with the main camera + # Pivot a back and forth to find rover + # FIXME: I think we can delete this + elif times_not_found == -1: if math.ceil(int(count / 20) / 5) % 2 == 1: self.speeds = [self.base_speed + 5, -self.base_speed - 5] else: @@ -219,10 +229,10 @@ def track_AR_Marker(self, aruco_id1: int, aruco_id2: int = -1): times_not_found < 15 ): # Lost the tag for less than 1.5 seconds after seeing it with the main camera times_not_found += 1 - print(f"lost tag {times_not_found} times") + logger.warning(f"lost tag {times_not_found} times") else: self.speeds = [0, 0] - print("lost it") # TODO this is bad + logger.warning("lost it") # TODO this is bad times_not_found = -1 # return False self.print_speeds() @@ -234,7 +244,7 @@ def track_AR_Marker(self, aruco_id1: int, aruco_id2: int = -1): self.error_accumulation = 0 print("Locked on and ready to track") - # Tracks down the tag + # Start moving towards the aruco tag while ( self.tracker.distance_to_marker > stop_distance or self.tracker.distance_to_marker == -1 From 39fa541687f6064b49eaf2f9f39684b6057d5a3b Mon Sep 17 00:00:00 2001 From: REMI Date: Wed, 22 May 2024 12:48:36 -0500 Subject: [PATCH 27/29] fix(aruco_tracker.py): rename clashing namespace items --- libs/aruco_tracker.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libs/aruco_tracker.py b/libs/aruco_tracker.py index 2e18ace..5273ec9 100755 --- a/libs/aruco_tracker.py +++ b/libs/aruco_tracker.py @@ -110,7 +110,7 @@ def __init__( # id1 is the main ar tag to telating to id2, since gateposts are no longrack, id2 is not relevant, image is the image to analyze # TODO: Get rid of anything relating to id2 # markerFound - def marker_found(self, id1: int, image, id2: int = -1) -> bool: + def is_marker_found(self, id1: int, image, id2: int = -1) -> bool: """ Attempts to find a marker with the given `id1` in the provided `image`. @@ -223,7 +223,7 @@ def find_marker(self, id1: int, id2: int = -1, cameras: int = -1) -> bool: for i in range(cameras): ret, frame = self.caps[i].read() - if self.marker_found(id1, frame, id2=id2): + if self.is_marker_found(id1, frame, id2=id2) == True: return True return False From adfa7ca32dbeda4124f4dba149e5bf11e124931d Mon Sep 17 00:00:00 2001 From: REMI Date: Wed, 22 May 2024 12:50:35 -0500 Subject: [PATCH 28/29] chore(pyproject.toml): default to Python@3.6 --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index b9e0148..413804a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -9,7 +9,7 @@ dependencies = [ "loguru>=0.7.2", ] readme = "README.md" -requires-python = ">= 3.10" +requires-python = ">= 3.6" [build-system] requires = ["hatchling"] build-backend = "hatchling.build" From 852f21ff616a9db11aca6036deaa1fdc6a1a7119 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 13:40:40 -0500 Subject: [PATCH 29/29] fix(ci): small Ruff lints --- libs/aruco_tracker.py | 2 +- libs/drive.py | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/libs/aruco_tracker.py b/libs/aruco_tracker.py index 5273ec9..abf00e3 100755 --- a/libs/aruco_tracker.py +++ b/libs/aruco_tracker.py @@ -223,7 +223,7 @@ def find_marker(self, id1: int, id2: int = -1, cameras: int = -1) -> bool: for i in range(cameras): ret, frame = self.caps[i].read() - if self.is_marker_found(id1, frame, id2=id2) == True: + if self.is_marker_found(id1, frame, id2=id2): return True return False diff --git a/libs/drive.py b/libs/drive.py index 5cf5c1e..c69f30b 100755 --- a/libs/drive.py +++ b/libs/drive.py @@ -2,10 +2,8 @@ from threading import Timer import configparser import os -from typing import Tuple import math from time import sleep -import sys from loguru import logger # FIXME(bray): we should be building rovermap into a package. that'd make importing easier