From 14445f682eef5719003b2e60105c1376c452cac3 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 17:54:54 -0500 Subject: [PATCH 01/43] feat(config.py): add `Config` type This allows folks to just do `config.stuff()` instead of fallibly-indexing into the ConfigParser dictionary each time. In other words, there's a single point of failure, which is something I'd like to get rid of. --- libs/config.py | 99 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 99 insertions(+) create mode 100644 libs/config.py diff --git a/libs/config.py b/libs/config.py new file mode 100644 index 0000000..677c3c9 --- /dev/null +++ b/libs/config.py @@ -0,0 +1,99 @@ +from configparser import ConfigParser + +from dataclasses import dataclass + +@dataclass(kw_only=True) +class Config: + """ + A container for the rover's configuration. + """ + config: ConfigParser + + ##### MBED, for controlling the LEDs ##### + + def mbed_ip(self) -> str: + """ + Returns the IP address of the MBED micro-controller. + """ + return str(self.config["CONFIG"]["MBED_IP"]) + + def mbed_port(self) -> int: + """ + Returns the port number of the MBED micro-controller. + """ + return int(self.config["CONFIG"]["MBED_PORT"]) + + ##### Swift, for getting GPS coordinates ##### + + def gps_ip(self) -> str: + """ + Returns the IP address of the Swift GPS module. + """ + return str(self.config["CONFIG"]["SWIFT_IP"]) + + def gps_port(self) -> int: + """ + Returns the port number of the Swift GPS module. + """ + return int(self.config["CONFIG"]["SWIFT_PORT"]) + + ##### OpenCV format ##### + + def opencv_format(self) -> str: + """ + Returns the OpenCV format string for camera initialization. + """ + return self.config["ARTRACKER"]["FORMAT"] + + ##### Camera traits, for object distances and angles ##### + + def camera_hdegrees_per_pixel(self) -> float: + """ + Returns the horizontal degrees per pixel for the camera. + """ + return float(self.config["ARTRACKER"]["DEGREES_PER_PIXEL"]) + + def camera_vdegrees_per_pixel(self) -> float: + """ + Returns the vertical degrees per pixel for the camera. + """ + return float(self.config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) + + def camera_frame_width(self) -> int: + """ + Returns the width of the camera frame. + """ + return int(self.config["ARTRACKER"]["FRAME_WIDTH"]) + + def camera_frame_height(self) -> int: + """ + Returns the height of the camera frame. + """ + return int(self.config["ARTRACKER"]["FRAME_HEIGHT"]) + + def camera_focal_length(self) -> float: + """ + The focal length of the camera. + """ + return float(self.config["ARTRACKER"]["FOCAL_LENGTH"]) + + ## TODO: remove these if we use the OpenCV distance/angle function + + def camera_known_marker_size(self) -> float: + """ + The real-world width/height of an ARUCO marker. This helps calculate + the distance/angle to the marker. + """ + return float(self.config["ARTRACKER"]["KNOWN_TAG_WIDTH"]) + + def camera_focal_length30h(self) -> float: + """ + The focal length of the camera at 30 degrees horizontal. + """ + return float(self.config["ARTRACKER"]["FOCAL_LENGTH30H"]) + + def camera_focal_length30v(self) -> float: + """ + The focal length of the camera at 30 degrees vertical. + """ + return float(self.config["ARTRACKER"]["FOCAL_LENGTH30V"]) \ No newline at end of file From d4f88a8c5abfb91cfefe37f3e70ae438c0e90d91 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 18:03:33 -0500 Subject: [PATCH 02/43] feat(communication.py): initial implementation --- libs/communication.py | 120 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 120 insertions(+) create mode 100644 libs/communication.py diff --git a/libs/communication.py b/libs/communication.py new file mode 100644 index 0000000..d1d3ba3 --- /dev/null +++ b/libs/communication.py @@ -0,0 +1,120 @@ +from dataclasses import dataclass +from socket import socket as Socket # fixes namespace collisions +from loguru import logger + + +@dataclass(kw_only=True) +class Communication: + """ + Communicate wheel speeds and LED commands to proper micro-controllers + """ + + socket: Socket + + # constant values to define wheel speeds + WHEEL_SUBSYSTEM_BYTE = 0x01 + WHEEL_PART_BYTE = 0x01 + + # LED subsystem constants + LED_SUBSYSTEM_BYTE = 0x01 + LED_PART_BYTE = 0x02 + + def __init__(self, ip: str, port: int): + self.socket = Socket(Socket.AF_INET, Socket.SOCK_DGRAM) + self.socket.connect((ip, port)) # adds the ip/port to socket's internal state + + def send_simple_wheel_speeds(self, left_speed: int, right_speed: int): + """ + Sends the given wheel side speeds to the rover. In this case, each + side uses the same speed for all their wheels. + """ + + def fix(speed: int) -> int: + # TODO: sanity check + fixed_speed = int((speed / 91.0) * 126) + + if fixed_speed < 0 or fixed_speed > 255: + logger.error("HEY, fixed speed is wrong! `{speed}` must be in [0, 255]") + + return fixed_speed + + (ls, rs) = (fix(left_speed), fix(right_speed)) + self.send_wheel_speeds(ls, ls, ls, rs, rs, rs) + + def send_wheel_speeds( + self, + front_left: int, + middle_left: int, + rear_left: int, + front_right: int, + middle_right: int, + rear_right: int, + ): + """ + Sends the given wheel speeds to the rover. Each wheel speed goes from + 0 to 255 (u8::MAX), with 126 being neutral. + """ + message: bytearray = bytearray(9) # the wheels use 9 bytes + + # add subsystem and part bytes + message.extend(self.WHEEL_SUBSYSTEM_BYTE, self.WHEEL_PART_BYTE) + + # stick in the speeds + message.extend( + [front_left, middle_left, rear_left, front_right, middle_right, rear_right] + ) + + # compute + add checksum + checksum = self.__checksum(message) + message.extend(checksum) + + # send the message over udp 🥺 + # TODO: consider using Google's QUIC instead! + self.socket.sendall(message) + logger.debug(f"Sending wheel speeds: {self.__prettyprint_byte_array(message)}") + + # Send LED Command + def send_led_command(self, red: int, green: int, blue: int): + """ + Sends the given LED color to the rover. All colors should be in the + range of [0, 255]. + + These are not checksummed. + """ + message: bytearray = bytearray(5) # LEDs use 5 bytes + message.extend(self.LED_SUBSYSTEM_BYTE, self.LED_PART_BYTE) + message.extend(red, green, blue) + self.socket.sendall(message) + + pass + + def send_led_red(self): + """Makes the LEDs red.""" + self.send_led_command(255, 0, 0) + + def send_led_green(self): + """Makes the LEDs green.""" + self.send_led_command(0, 255, 0) + + def send_led_blue(self): + """Makes the LEDs blue.""" + self.send_led_command(0, 0, 255) + + def __checksum(self, byte_array: list[int]) -> int: + """ + Calculates the checksum of a byte array. + """ + checksum: int = sum(byte_array) & 0xFF + return checksum + + def __prettyprint_byte_array(self, byte_array: list[int]): + """ + Prints a byte array in a human-readable format. + """ + pretty_print: str = "[" + for index, byte in enumerate(byte_array): + pretty_print += f"0x{byte:02X}" + if index != len(byte_array) - 1: + pretty_print += ", " + pretty_print += "]" + print(pretty_print) From 708ed973493fe357c06facf8a9f833a1582ea4f3 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 19:22:49 -0500 Subject: [PATCH 03/43] feat(navigation): ok yeah i refactored location.py this one uses a library to do the grunt work (math), allowing for cleaner, less scary code it also creates two files. let me know if that looks bad or something --- libs/gps_controller.py | 52 ++++++++++++++++++ libs/navigation.py | 116 +++++++++++++++++++++++++++++++++++++++++ pyproject.toml | 2 + 3 files changed, 170 insertions(+) create mode 100644 libs/gps_controller.py create mode 100644 libs/navigation.py diff --git a/libs/gps_controller.py b/libs/gps_controller.py new file mode 100644 index 0000000..bbe5553 --- /dev/null +++ b/libs/gps_controller.py @@ -0,0 +1,52 @@ +from dataclasses import dataclass +from time import sleep + +from loguru import logger + +from gps import gps +from libs import config, navigation +from libs.navigation import Coordinate + +@dataclass(kw_only=True) +class GpsController: + """ + Controls the GPS library. + """ + + enabled: bool = False # whether or not gps comms are enabled + config: config.Config + + coordinates: Coordinate = Coordinate(0.0, 0.0) + height: float = 0.0 # in meters + time: float = 0.0 # "time of week" in milliseconds + error: float = 0.0 # in millimeters, accounting for vert/horiz error + angle: float = 0.0 # in degrees + previous_coordinates: Coordinate = Coordinate(0.0, 0.0) + + SLEEP_TIME: float = 0.1 + + def __init__(self, swift_ip: int, swift_port: int): + """ + Initialize Location object to get coordinates + """ + pass + + def check(self): + while self.enabled: + if gps.get_latitude() + gps.get_longitude() == 0: + logger.error("Expected GPS data but got none.") + else: + # we've got GPS data. let's do our thing... + # make current coords old + self.previous_coordinates = self.coordinates + + # get new values + self.coordinates = Coordinate(gps.get_latitude(), gps.get_longitude()) + self.height = gps.get_height() + self.time = gps.get_time() + self.error = gps.get_error() + + # calculate angle ("bearing") between new and old coordinates + self.angle = navigation.calculate_angle(self.coordinates, self.previous_coordinates) + + sleep(self.SLEEP_TIME) \ No newline at end of file diff --git a/libs/navigation.py b/libs/navigation.py new file mode 100644 index 0000000..a242bc3 --- /dev/null +++ b/libs/navigation.py @@ -0,0 +1,116 @@ +from dataclasses import dataclass + +from geographiclib.geodesic import Geodesic +from loguru import logger + +from libs import config +from libs.gps_controller import GpsController + + +@dataclass() +class Coordinate: + latitude: float + longitude: float + + def latitude(self) -> float: + self.latitude + + def longitude(self) -> float: + self.longitude + + def set_latitude(self, latitude: float): + self.latitude = latitude + + def set_longitude(self, longitude: float): + self.longitude = longitude + + def __str__(self): + return f"({self.latitude}, {self.longitude})" + + +@dataclass(kw_only=True) +class Navigation: + """ + Keeps track of latititude and longitude, distance to objective, and angle to objective. + + It also calculates some of these values. + + - `rover_coords`: location of the rover + - `given_coords`: a given gps coordinate to navigate to. Depending on + section of comp, ARUCO tags will be nearby + - `distance_to_objective`: distance in centimeters + - `angle_to_objective`: from the rover to objective in degrees + """ + + config: config.Config + rover_coords: Coordinate + given_coords: Coordinate + distance_to_objective: float + angle_to_objective: float + gps: GpsController + + def __init__(self, swift_ip: int, swift_port: int): + """ + Initialize Location object to get coordinates + """ + pass + + def distance_to_object(self, coord: Coordinate) -> float: + """ + Finds the distance between the rover and `coord`. + """ + earth: Geodesic = Geodesic.WGS84 + res = earth.Inverse( + self.rover_coords.latitude(), + self.rover_coords.longitude(), + coord.latitude(), + coord.longitude(), + ) + distance: float = res["s12"] + + logger.debug( + f"Distance between `{self.rover_coords}` and `{coord}`: `{distance}`m" + ) + return distance + + def angle_to_object(self, coord: Coordinate) -> float: + """ + Finds the angle between the rover and `coord`. + """ + return calculate_angle(self.rover_coords, coord) + + def coordinates_to_object(self, distance_km: float, angle_deg: float) -> Coordinate: + """ + Calculates the coordinates of another object compared to the Rover. + + - `distance_km`: The object's distance from the Rover in kilometers. + - `angle_deg`: The object's angle from the Rover in degrees. + """ + earth: Geodesic = Geodesic.WGS84 + res = earth.Direct( + distance_km, + angle_deg, + self.rover_coords.latitude(), + self.rover_coords.longitude(), + ) + c = Coordinate(res["lat2"], res["lon2"]) + + logger.debug(f"Object coordinates (lat, lon): {c}") + return c + + +def calculate_angle(self, co1: Coordinate, co2: Coordinate) -> float: + """ + Calculates the angle between two coordinates. + + - `co1`: first coordinate. + - `co2`: second coordinate. + """ + earth: Geodesic = Geodesic.WGS84 + res = earth.Inverse( + co1.latitude(), co1.longitude(), co2.latitude(), co2.longitude() + ) + angle: float = res["azi1"] + + logger.debug(f"Angle between `{co1}` and `{co2}`: `{angle}`°") + return angle diff --git a/pyproject.toml b/pyproject.toml index 413804a..2b3684a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,6 +7,8 @@ dependencies = [ "opencv-python>=4.7", "flask>=3.0.3", "loguru>=0.7.2", + "geographiclib>=2.0", + # "dataclasses==0.8", # ATTENTION: this is required on the Rover for python 3.6 ] readme = "README.md" requires-python = ">= 3.6" From 94ce345cf08a01b6c708506c8374d35290a174a6 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 19:24:36 -0500 Subject: [PATCH 04/43] fix(communication.py): use more obvious arg names --- libs/communication.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libs/communication.py b/libs/communication.py index d1d3ba3..1e6941c 100644 --- a/libs/communication.py +++ b/libs/communication.py @@ -1,5 +1,6 @@ from dataclasses import dataclass from socket import socket as Socket # fixes namespace collisions + from loguru import logger @@ -19,9 +20,9 @@ class Communication: LED_SUBSYSTEM_BYTE = 0x01 LED_PART_BYTE = 0x02 - def __init__(self, ip: str, port: int): + def __init__(self, rover_ip: str, rover_port: int): self.socket = Socket(Socket.AF_INET, Socket.SOCK_DGRAM) - self.socket.connect((ip, port)) # adds the ip/port to socket's internal state + self.socket.connect((rover_ip, rover_port)) # adds ip/port to socket's state def send_simple_wheel_speeds(self, left_speed: int, right_speed: int): """ From cd0241fc507bfa3e9bdf1a86cef9fe9821b6aebd Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 19:34:50 -0500 Subject: [PATCH 05/43] feat(gps_controller.py): add start/stop methods these replace Location.start/stop(_gps) :3 --- libs/gps_controller.py | 38 ++++++++++++++++++++++++++++++++------ libs/navigation.py | 3 ++- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/libs/gps_controller.py b/libs/gps_controller.py index bbe5553..3762031 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -1,4 +1,5 @@ from dataclasses import dataclass +from threading import Thread from time import sleep from loguru import logger @@ -7,6 +8,7 @@ from libs import config, navigation from libs.navigation import Coordinate + @dataclass(kw_only=True) class GpsController: """ @@ -31,22 +33,46 @@ def __init__(self, swift_ip: int, swift_port: int): """ pass + def start(self): + """ + Starts the GPS thread and connects to the GPS hardware itself. + """ + gps.gps_init(self.config.swift_ip(), self.config.swift_port()) + self.enabled = True + + logger.info("Starting GPS thread...") + t = Thread(target=self.check, name=("GPS thread"), args=()) + t.start() + logger.info("GPS thread started!") + pass + + def stop(self): + """ + Sets the GPS state to be off, causing the GPS thread to stop. + """ + self.enabled = False + gps.gps_finish() + def check(self): while self.enabled: if gps.get_latitude() + gps.get_longitude() == 0: logger.error("Expected GPS data but got none.") - else: + else: # we've got GPS data. let's do our thing... # make current coords old self.previous_coordinates = self.coordinates - + # get new values self.coordinates = Coordinate(gps.get_latitude(), gps.get_longitude()) self.height = gps.get_height() self.time = gps.get_time() self.error = gps.get_error() - + # calculate angle ("bearing") between new and old coordinates - self.angle = navigation.calculate_angle(self.coordinates, self.previous_coordinates) - - sleep(self.SLEEP_TIME) \ No newline at end of file + self.angle = navigation.calculate_angle( + self.coordinates, self.previous_coordinates + ) + + sleep(self.SLEEP_TIME) + + logger.info("GPS thread: no longer enabled, shutting down...") diff --git a/libs/navigation.py b/libs/navigation.py index a242bc3..b1cea65 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -51,8 +51,9 @@ class Navigation: def __init__(self, swift_ip: int, swift_port: int): """ - Initialize Location object to get coordinates + Initializes the navigation to get coordinates. """ + # TODO pass def distance_to_object(self, coord: Coordinate) -> float: From 7dba73670c72fdb32ca1d0adb886f08ed6f9eaa7 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 19:46:15 -0500 Subject: [PATCH 06/43] fix(Navigation): access the GPS --- libs/navigation.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/libs/navigation.py b/libs/navigation.py index b1cea65..4f019c8 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -3,7 +3,7 @@ from geographiclib.geodesic import Geodesic from loguru import logger -from libs import config +from libs.config import Config from libs.gps_controller import GpsController @@ -42,19 +42,20 @@ class Navigation: - `angle_to_objective`: from the rover to objective in degrees """ - config: config.Config - rover_coords: Coordinate + config: Config given_coords: Coordinate - distance_to_objective: float - angle_to_objective: float - gps: GpsController + gps: GpsController | None = None - def __init__(self, swift_ip: int, swift_port: int): + def __init__( + self, config: Config, given_coords: Coordinate, swift_ip: int, swift_port: int + ): """ Initializes the navigation to get coordinates. """ - # TODO - pass + self.given_coords = given_coords + self.config = config + self.gps = GpsController(swift_ip, swift_port) + self.gps.start() def distance_to_object(self, coord: Coordinate) -> float: """ From 803093d5be52960d9d8576e1316bc7c276560d86 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 19:46:56 -0500 Subject: [PATCH 07/43] feat(Navigation): include finish method this should always be called! otherwise, the GPS might be left in a weird state... --- libs/navigation.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/libs/navigation.py b/libs/navigation.py index 4f019c8..3a1c7fd 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -34,6 +34,8 @@ class Navigation: Keeps track of latititude and longitude, distance to objective, and angle to objective. It also calculates some of these values. + + The rover should run the `finish` method when it is done navigating. - `rover_coords`: location of the rover - `given_coords`: a given gps coordinate to navigate to. Depending on @@ -57,6 +59,15 @@ def __init__( self.gps = GpsController(swift_ip, swift_port) self.gps.start() + def finish(self): + """ + Stops the GPS thread and shuts down the GPS controller. + + This should always be called when the rover finishes navigation. + """ + self.gps.stop() + self.gps = None + def distance_to_object(self, coord: Coordinate) -> float: """ Finds the distance between the rover and `coord`. From c4533715ffb708b169eda0fb72d6554eb626ef79 Mon Sep 17 00:00:00 2001 From: Barrett Date: Wed, 22 May 2024 22:57:25 -0500 Subject: [PATCH 08/43] =?UTF-8?q?a=20wip=20commit.=20don=E2=80=99t=20keep?= =?UTF-8?q?=20in=20tree?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit yeah here's a dump of all the stuff in case i lose it or something --- libs/aruco_tracker.py | 265 +++++++++-------------------------------- libs/gps_controller.py | 17 +++ libs/navigation.py | 5 +- libs/rover.py | 112 +++++++++++++++++ 4 files changed, 187 insertions(+), 212 deletions(-) create mode 100644 libs/rover.py diff --git a/libs/aruco_tracker.py b/libs/aruco_tracker.py index abf00e3..d52144f 100755 --- a/libs/aruco_tracker.py +++ b/libs/aruco_tracker.py @@ -1,229 +1,78 @@ -import configparser -import os -from time import sleep -from typing import List +from dataclasses import dataclass import cv2 -import cv2.aruco as aruco from loguru import logger -# TODO(bray): `dataclasses` -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, - 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 - self.index1 = -1 - self.index2 = -1 - self.cameras = cameras +@dataclass(kw_only=True) +class ArucoTracker: + """ + Capture frames and detect ARUCO markers in frames + """ - # 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:") - if os.path.isabs(config_file): - print(config_file) # print the absolute path - else: - # print the full relative path - print(f"{os.getcwd()}/{config_file}") - exit(-2) - - # Set variables from the config file - 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"]) - - # Initialize video writer, fps is set to 5 - if self.write: - 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 ARUCO marker dictionary - self.marker_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) - - # Initialize cameras - # TODO: Could this be in a function - 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 - while True: - cam = cv2.VideoCapture(self.cameras[i]) - 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) - - # 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] - ), - ) - # 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: # if we did get a frame... - self.caps.append(cam) # add to our list of cameras - break + aruco_marker: int + marker_size: int # In centimeters + # TODO: Find out how to get camera matrix and distance coefficients + camera_mtx: type # TODO + distance_coefficients: type # TODO + aruco_dictionary: cv2.aruco.Dictionary + video_capture: cv2.VideoCapture - # 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 is_marker_found(self, id1: int, image, id2: int = -1) -> bool: + def __init__(self, opencv_camera_index: int): """ - Attempts to find a marker with the given `id1` in the provided `image`. - - Returns true if found. + Create and configure `VideoCapture` object from opencv camera index + Create and initialize aruco dictionary """ + pass - found = False + def find_marker(self) -> None | tuple[float, float]: + """ + Captures a frame and checks to see if `aruco_marker` is present. - # converts to grayscale - grayscale = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) + Either returns `None` if not found, or a tuple (a: float, b: float): + - `a`: the distance to the marker in centimeters + - `b`: the angle to the marker in degrees + """ - self.index1 = -1 - self.index2 = -1 - 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! - bw = cv2.threshold(grayscale, i, 255, cv2.THRESH_BINARY)[1] - (self.corners, self.marker_IDs, self.rejected) = aruco.detectMarkers( + # Capture a new frame + ret, frame = self.video_capture.read() + if not ret: + logger.error("FRAME COULDNT BE CAPTURED! Camera may be busy.") + return None + + # Convert frame to grayscale and try to find find a marker + grayscale = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY, frame) + for threshold_cutoff in range( + 40, 221, 60 + ): # Check for arucos tags with different thresholds + # Find aruco tags in image with given threshold cutoff + bw = cv2.threshold(grayscale, threshold_cutoff, 255, cv2.THRESH_BINARY)[1] + (tag_corners, aruco_tags, __) = cv2.aruco.detectMarkers( bw, self.marker_dict ) - if self.marker_IDs is not None: - 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: # 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) + # check if our tag is in the list of found aruco tags + tag_index: int | None = None + for tag in aruco_tags: + if tag == self.aruco_marker: + logger.info(f"ArUco tag `{tag}` found!") + tag_index = tag break - # 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] + # if so, let's estimate its pose + if tag_index is not None: + logger.debug("Checking tag pose...") + rotation_vects, translation_vects = cv2.aruco.estimatePoseSingleMarkers( + tag_corners[tag_index], + self.marker_size, + self.camera_mtx, + self.distance_coefficients, ) - + ( - 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: int = -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) + # calculations to get distance and angle to marker + distance_to_marker, angle_to_marker = 0.0 - for i in range(cameras): - ret, frame = self.caps[i].read() - if self.is_marker_found(id1, frame, id2=id2): - return True + # TODO + return (distance_to_marker, angle_to_marker) - return False + return None diff --git a/libs/gps_controller.py b/libs/gps_controller.py index 3762031..4402ef5 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -18,6 +18,7 @@ class GpsController: enabled: bool = False # whether or not gps comms are enabled config: config.Config + # note: if you change these, make sure to go to GpsReport too coordinates: Coordinate = Coordinate(0.0, 0.0) height: float = 0.0 # in meters time: float = 0.0 # "time of week" in milliseconds @@ -72,7 +73,23 @@ def check(self): self.angle = navigation.calculate_angle( self.coordinates, self.previous_coordinates ) + + # TODO: make report and send to navigation thread sleep(self.SLEEP_TIME) logger.info("GPS thread: no longer enabled, shutting down...") + +@dataclass(kw_only=True) +class GpsReport: + """ + A container for GPS data. Sendable across threads. + """ + + coordinates: Coordinate = Coordinate(0.0, 0.0) + height: float = 0.0 # in meters + time: float = 0.0 # "time of week" in milliseconds + error: float = 0.0 # in millimeters, accounting for vert/horiz error + angle: float = 0.0 # in degrees + previous_coordinates: Coordinate = Coordinate(0.0, 0.0) + \ No newline at end of file diff --git a/libs/navigation.py b/libs/navigation.py index 3a1c7fd..e469e98 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -37,11 +37,8 @@ class Navigation: The rover should run the `finish` method when it is done navigating. - - `rover_coords`: location of the rover - `given_coords`: a given gps coordinate to navigate to. Depending on section of comp, ARUCO tags will be nearby - - `distance_to_objective`: distance in centimeters - - `angle_to_objective`: from the rover to objective in degrees """ config: Config @@ -126,4 +123,4 @@ def calculate_angle(self, co1: Coordinate, co2: Coordinate) -> float: angle: float = res["azi1"] logger.debug(f"Angle between `{co1}` and `{co2}`: `{angle}`°") - return angle + return angle \ No newline at end of file diff --git a/libs/rover.py b/libs/rover.py new file mode 100644 index 0000000..3b793ed --- /dev/null +++ b/libs/rover.py @@ -0,0 +1,112 @@ +import configparser +import os +from dataclasses import dataclass +from threading import Thread +from time import sleep + +import cv2 + +from libs import config +from libs.communication import Communication +from libs.navigation import Coordinate, Navigation +from maps import MapServer + +@dataclass +class Mode: + SEARCHING_ALONG_COORDINATE = 0 + ARUCO = 1 + YOLO = 2 + + state: int + + +@dataclass(kw_only=True) +class Rover: + """ + A representation of the rover. + """ + + conf: config.Config + rover_connection: Communication + opencv_camera_index: int + nav: Navigation + mode: Mode + + def __init__(self, given_coords: Coordinate, opencv_camera_index: int): + # sets up the parser + config = configparser.ConfigParser(allow_no_value=True) + config.read(os.path.dirname(__file__) + "/../config.ini") + + # initialize the gps object + # this starts a thread for the gps + self.nav = Navigation( + config, + given_coords, + swift_ip=self.conf.swift_ip(), + swift_port=self.conf.swift_port(), + ) + self.gps.start_GPS() + + self.speeds = [0, 0] + self.error_accumulation = 0.0 + + # TODO: Initiate different threads + # ✅ Navigation thread + # - Communication thread + # - Aruco Tracker/YOLO thread + # - RoverMap Thread + + self.opencv_camera_index = opencv_camera_index + self.rover_connection = Communication( + self.conf.swift_ip(), self.conf.swift_port() + ) + + pass + + def start_navigation_thread(self): + # What does navigati + pass + + def start_communication_thread(self): + # starts the thread that sends wheel speeds + self.running = True + t = Thread(target=self.send_speed, name=("send wheel speeds"), args=()) + t.daemon = True + t.start() + pass + + def start_aruco_tracker_thread(self): + pass + + def start_yolo_thread(self): + pass + + def start_rover_map(self): + # 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) + pass + + def pid_controller(self): + """ + Read from navigation thread and send commands through communication thread + """ + pass + + +@dataclass(kw_only=True) +class Yolo: + """ + Captures frames and checks for competition objects in a frame. Computes bounding boxes. + """ + + aruco_marker: int + video_capture: cv2.VideoCapture + + def __init__(self): + """ + Initialize YOLO detection model + """ From 78fe5f52dd08007574a48a192f2d6d7fab582e56 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:19:07 -0500 Subject: [PATCH 09/43] fix: Use Python 3.7 syntax This may not work with 3.6 but is guaranteed to work in 3.11 and 3.7, the other two versions that we have access to Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/aruco_tracker.py | 9 +++++---- libs/communication.py | 6 +++--- libs/config.py | 3 ++- libs/gps_controller.py | 3 ++- pyproject.toml | 4 ++-- requirements-dev.lock | 38 ++++++++++++++++++++++++++++++++++++++ requirements.lock | 38 ++++++++++++++++++++++++++++++++++++++ 7 files changed, 90 insertions(+), 11 deletions(-) create mode 100644 requirements-dev.lock create mode 100644 requirements.lock diff --git a/libs/aruco_tracker.py b/libs/aruco_tracker.py index d52144f..878fea2 100755 --- a/libs/aruco_tracker.py +++ b/libs/aruco_tracker.py @@ -1,10 +1,11 @@ from dataclasses import dataclass +from typing import Tuple, Union import cv2 from loguru import logger -@dataclass(kw_only=True) +@dataclass() class ArucoTracker: """ Capture frames and detect ARUCO markers in frames @@ -25,7 +26,7 @@ def __init__(self, opencv_camera_index: int): """ pass - def find_marker(self) -> None | tuple[float, float]: + def find_marker(self) -> Union[None, Tuple[float, float]]: """ Captures a frame and checks to see if `aruco_marker` is present. @@ -52,7 +53,7 @@ def find_marker(self) -> None | tuple[float, float]: ) # check if our tag is in the list of found aruco tags - tag_index: int | None = None + tag_index: Union[int, None] = None for tag in aruco_tags: if tag == self.aruco_marker: logger.info(f"ArUco tag `{tag}` found!") @@ -70,7 +71,7 @@ def find_marker(self) -> None | tuple[float, float]: ) # calculations to get distance and angle to marker - distance_to_marker, angle_to_marker = 0.0 + (distance_to_marker, angle_to_marker) = (0.0, 0.0) # TODO return (distance_to_marker, angle_to_marker) diff --git a/libs/communication.py b/libs/communication.py index 1e6941c..8a428f5 100644 --- a/libs/communication.py +++ b/libs/communication.py @@ -4,7 +4,7 @@ from loguru import logger -@dataclass(kw_only=True) +@dataclass() class Communication: """ Communicate wheel speeds and LED commands to proper micro-controllers @@ -101,14 +101,14 @@ def send_led_blue(self): """Makes the LEDs blue.""" self.send_led_command(0, 0, 255) - def __checksum(self, byte_array: list[int]) -> int: + def __checksum(self, byte_array: bytearray) -> int: """ Calculates the checksum of a byte array. """ checksum: int = sum(byte_array) & 0xFF return checksum - def __prettyprint_byte_array(self, byte_array: list[int]): + def __prettyprint_byte_array(self, byte_array: bytearray) -> str: """ Prints a byte array in a human-readable format. """ diff --git a/libs/config.py b/libs/config.py index 677c3c9..2ddd13d 100644 --- a/libs/config.py +++ b/libs/config.py @@ -2,7 +2,8 @@ from dataclasses import dataclass -@dataclass(kw_only=True) + +@dataclass() class Config: """ A container for the rover's configuration. diff --git a/libs/gps_controller.py b/libs/gps_controller.py index 4402ef5..d515c0c 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -9,7 +9,7 @@ from libs.navigation import Coordinate -@dataclass(kw_only=True) +@dataclass() class GpsController: """ Controls the GPS library. @@ -81,6 +81,7 @@ def check(self): logger.info("GPS thread: no longer enabled, shutting down...") @dataclass(kw_only=True) +@dataclass() class GpsReport: """ A container for GPS data. Sendable across threads. diff --git a/pyproject.toml b/pyproject.toml index 2b3684a..12e28bf 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -5,7 +5,7 @@ description = "Add your description here" authors = [{ name = "Barrett", email = "contact@barretts.club" }] dependencies = [ "opencv-python>=4.7", - "flask>=3.0.3", + "flask>=2.0", "loguru>=0.7.2", "geographiclib>=2.0", # "dataclasses==0.8", # ATTENTION: this is required on the Rover for python 3.6 @@ -18,7 +18,7 @@ build-backend = "hatchling.build" [tool.rye] managed = true -dev-dependencies = ["mypy>=1.8.0", "ruff>=0.2.0"] +#dev-dependencies = ["mypy>=1.4.0", "ruff>=0.2.0"] [tool.hatch.metadata] allow-direct-references = true diff --git a/requirements-dev.lock b/requirements-dev.lock new file mode 100644 index 0000000..326676e --- /dev/null +++ b/requirements-dev.lock @@ -0,0 +1,38 @@ +# generated by rye +# use `rye lock` or `rye sync` to update this lockfile +# +# last locked with the following flags: +# pre: false +# features: [] +# all-features: false +# with-sources: false + +-e file:. +click==8.1.7 + # via flask +flask==2.2.5 + # via autonomous +geographiclib==2.0 + # via autonomous +importlib-metadata==6.7.0 + # via click + # via flask +itsdangerous==2.1.2 + # via flask +jinja2==3.1.4 + # via flask +loguru==0.7.2 + # via autonomous +markupsafe==2.1.5 + # via jinja2 + # via werkzeug +numpy==1.21.6 + # via opencv-python +opencv-python==4.9.0.80 + # via autonomous +typing-extensions==4.7.1 + # via importlib-metadata +werkzeug==2.2.3 + # via flask +zipp==3.15.0 + # via importlib-metadata diff --git a/requirements.lock b/requirements.lock new file mode 100644 index 0000000..326676e --- /dev/null +++ b/requirements.lock @@ -0,0 +1,38 @@ +# generated by rye +# use `rye lock` or `rye sync` to update this lockfile +# +# last locked with the following flags: +# pre: false +# features: [] +# all-features: false +# with-sources: false + +-e file:. +click==8.1.7 + # via flask +flask==2.2.5 + # via autonomous +geographiclib==2.0 + # via autonomous +importlib-metadata==6.7.0 + # via click + # via flask +itsdangerous==2.1.2 + # via flask +jinja2==3.1.4 + # via flask +loguru==0.7.2 + # via autonomous +markupsafe==2.1.5 + # via jinja2 + # via werkzeug +numpy==1.21.6 + # via opencv-python +opencv-python==4.9.0.80 + # via autonomous +typing-extensions==4.7.1 + # via importlib-metadata +werkzeug==2.2.3 + # via flask +zipp==3.15.0 + # via importlib-metadata From d7b9d0d8bb1dd71523730c7b4250f2ba7744655a Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:19:22 -0500 Subject: [PATCH 10/43] chore(temp): add explanation on the true bearing Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- explain | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 explain diff --git a/explain b/explain new file mode 100644 index 0000000..765de25 --- /dev/null +++ b/explain @@ -0,0 +1,12 @@ +calc_bearing function: Calulates the `true bearing` for the rover + +- true bearing is the bearing from North + +bearing_to: This calculates the `relative bearing` between the rover and a coordinate + +- This would be the difference between the rover's tru bearing and the coords true bearing +- relative bearing is the bearing between any two objects + +gps_controller we need calc_bearing + +navigation we need bearing_to From 8e8b4f97a9603a961600e12c92288ed1756815f2 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:19:50 -0500 Subject: [PATCH 11/43] chore(Config): formatting Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/config.py | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/libs/config.py b/libs/config.py index 2ddd13d..3c3608e 100644 --- a/libs/config.py +++ b/libs/config.py @@ -8,46 +8,47 @@ class Config: """ A container for the rover's configuration. """ + config: ConfigParser - + ##### MBED, for controlling the LEDs ##### - + def mbed_ip(self) -> str: """ Returns the IP address of the MBED micro-controller. """ return str(self.config["CONFIG"]["MBED_IP"]) - + def mbed_port(self) -> int: """ Returns the port number of the MBED micro-controller. """ return int(self.config["CONFIG"]["MBED_PORT"]) - + ##### Swift, for getting GPS coordinates ##### - + def gps_ip(self) -> str: """ Returns the IP address of the Swift GPS module. """ return str(self.config["CONFIG"]["SWIFT_IP"]) - + def gps_port(self) -> int: """ Returns the port number of the Swift GPS module. """ return int(self.config["CONFIG"]["SWIFT_PORT"]) - + ##### OpenCV format ##### - + def opencv_format(self) -> str: """ Returns the OpenCV format string for camera initialization. """ return self.config["ARTRACKER"]["FORMAT"] - + ##### Camera traits, for object distances and angles ##### - + def camera_hdegrees_per_pixel(self) -> float: """ Returns the horizontal degrees per pixel for the camera. @@ -59,7 +60,7 @@ def camera_vdegrees_per_pixel(self) -> float: Returns the vertical degrees per pixel for the camera. """ return float(self.config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) - + def camera_frame_width(self) -> int: """ Returns the width of the camera frame. @@ -71,18 +72,18 @@ def camera_frame_height(self) -> int: Returns the height of the camera frame. """ return int(self.config["ARTRACKER"]["FRAME_HEIGHT"]) - + def camera_focal_length(self) -> float: """ The focal length of the camera. """ return float(self.config["ARTRACKER"]["FOCAL_LENGTH"]) - + ## TODO: remove these if we use the OpenCV distance/angle function - + def camera_known_marker_size(self) -> float: """ - The real-world width/height of an ARUCO marker. This helps calculate + The real-world width/height of an ARUCO marker. This helps calculate the distance/angle to the marker. """ return float(self.config["ARTRACKER"]["KNOWN_TAG_WIDTH"]) @@ -97,4 +98,4 @@ def camera_focal_length30v(self) -> float: """ The focal length of the camera at 30 degrees vertical. """ - return float(self.config["ARTRACKER"]["FOCAL_LENGTH30V"]) \ No newline at end of file + return float(self.config["ARTRACKER"]["FOCAL_LENGTH30V"]) From dec5bbd9be8c720ae786c04d8df0044d881f2c80 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:20:28 -0500 Subject: [PATCH 12/43] chore(main): add wheels deadlock TODO Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- main.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/main.py b/main.py index df128c9..e9b5628 100755 --- a/main.py +++ b/main.py @@ -8,6 +8,8 @@ from pathlib import Path from configparser import ConfigParser +# TODO(rewrite): make sure the speeds are correct. account for wheel deadlock! + def flash(mbedIP: str, mbedPort: int) -> None: """ @@ -15,7 +17,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) @@ -23,7 +25,6 @@ def flash(mbedIP: str, mbedPort: int) -> None: sleep(0.2) - # Create Argument Parser and parse arguments arg_parser = argparse.ArgumentParser() arg_parser.add_argument( @@ -50,7 +51,7 @@ def flash(mbedIP: str, mbedPort: int) -> None: # TODO: dataclass this function. returning a tuple is ugly and begging for bugs # TODO: use typing. broken due to old ass python (<3.8) -#def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: +# def parse_arguments() -> Tuple[list[int], list[Tuple[float, float]]]: def parse_arguments(): """ Parses the command line arguments. @@ -103,8 +104,10 @@ def parse_arguments(): # TODO: make this a part of a `Rover` class :3 def drive( # 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 + # 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 @@ -114,9 +117,10 @@ def drive( # Drive along GPS coordinates 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]) + 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: From 83d5ef1acab23a442efb0680ca36092a0c238cc7 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:20:59 -0500 Subject: [PATCH 13/43] fix(Rover): use `gps_` over `swift_` Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/rover.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/libs/rover.py b/libs/rover.py index 3b793ed..8d84e04 100644 --- a/libs/rover.py +++ b/libs/rover.py @@ -11,16 +11,17 @@ from libs.navigation import Coordinate, Navigation from maps import MapServer + @dataclass class Mode: SEARCHING_ALONG_COORDINATE = 0 ARUCO = 1 YOLO = 2 - + state: int - -@dataclass(kw_only=True) + +@dataclass() class Rover: """ A representation of the rover. @@ -42,24 +43,22 @@ def __init__(self, given_coords: Coordinate, opencv_camera_index: int): self.nav = Navigation( config, given_coords, - swift_ip=self.conf.swift_ip(), - swift_port=self.conf.swift_port(), + swift_ip=self.conf.gps_ip(), + swift_port=self.conf.gps_port(), ) self.gps.start_GPS() self.speeds = [0, 0] self.error_accumulation = 0.0 - # TODO: Initiate different threads + # TODO: Initiate different threads # ✅ Navigation thread # - Communication thread # - Aruco Tracker/YOLO thread # - RoverMap Thread self.opencv_camera_index = opencv_camera_index - self.rover_connection = Communication( - self.conf.swift_ip(), self.conf.swift_port() - ) + self.rover_connection = Communication(self.conf.gps_ip(), self.conf.gps_port()) pass @@ -97,7 +96,7 @@ def pid_controller(self): pass -@dataclass(kw_only=True) +@dataclass() class Yolo: """ Captures frames and checks for competition objects in a frame. Computes bounding boxes. From a6256c7c355e846a406dcf9ded585856cc706667 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:21:33 -0500 Subject: [PATCH 14/43] fix(Nav): take IP an a str, not int Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/navigation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/navigation.py b/libs/navigation.py index e469e98..7fd9311 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -46,7 +46,7 @@ class Navigation: gps: GpsController | None = None def __init__( - self, config: Config, given_coords: Coordinate, swift_ip: int, swift_port: int + self, config: Config, given_coords: Coordinate, swift_ip: str, swift_port: int ): """ Initializes the navigation to get coordinates. From 5e1b7160460b4129f8d7b2c542cd010369c466f2 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:21:53 -0500 Subject: [PATCH 15/43] fix(GpsControl): take swift ip as a str Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/gps_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/gps_controller.py b/libs/gps_controller.py index d515c0c..d3e1345 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -28,7 +28,7 @@ class GpsController: SLEEP_TIME: float = 0.1 - def __init__(self, swift_ip: int, swift_port: int): + def __init__(self, swift_ip: str, swift_port: int): """ Initialize Location object to get coordinates """ From d22e21015919f094607a5a02cf0cb6ae95b7be7c Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:22:47 -0500 Subject: [PATCH 16/43] feat(GpsControl): use "true bearing" over "angle" True bearing represents something different. See the previous "chore(temp)" commit for additional information. Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/gps_controller.py | 46 ++++++++++++++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 11 deletions(-) diff --git a/libs/gps_controller.py b/libs/gps_controller.py index d3e1345..c427429 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -3,9 +3,10 @@ from time import sleep from loguru import logger +from geographiclib.geodesic import Geodesic from gps import gps -from libs import config, navigation +from libs import config from libs.navigation import Coordinate @@ -15,15 +16,15 @@ class GpsController: Controls the GPS library. """ + conf: config.Config enabled: bool = False # whether or not gps comms are enabled - config: config.Config # note: if you change these, make sure to go to GpsReport too coordinates: Coordinate = Coordinate(0.0, 0.0) height: float = 0.0 # in meters time: float = 0.0 # "time of week" in milliseconds error: float = 0.0 # in millimeters, accounting for vert/horiz error - angle: float = 0.0 # in degrees + true_bearing: float = 0.0 # Bearing from True North previous_coordinates: Coordinate = Coordinate(0.0, 0.0) SLEEP_TIME: float = 0.1 @@ -38,7 +39,7 @@ def start(self): """ Starts the GPS thread and connects to the GPS hardware itself. """ - gps.gps_init(self.config.swift_ip(), self.config.swift_port()) + gps.gps_init(self.conf.gps_ip(), self.conf.gps_port()) self.enabled = True logger.info("Starting GPS thread...") @@ -69,9 +70,9 @@ def check(self): self.time = gps.get_time() self.error = gps.get_error() - # calculate angle ("bearing") between new and old coordinates - self.angle = navigation.calculate_angle( - self.coordinates, self.previous_coordinates + # calculate true bearing between new and old coordinates + self.true_bearing = calculate_true_bearing( + self.previous_coordinates, self.coordinates ) # TODO: make report and send to navigation thread @@ -80,17 +81,40 @@ def check(self): logger.info("GPS thread: no longer enabled, shutting down...") -@dataclass(kw_only=True) + +def calculate_true_bearing(prev_coords: Coordinate, curr_coords: Coordinate) -> float: + """ + Calculate the True Bearing for an object given its current position and previous position. + NOTE: True bearing is the bearing relative to North + + - `co1`: first coordinate. + - `co2`: second coordinate. + """ + earth = Geodesic.WGS84 + + res = earth.Inverse( + prev_coords.get_latitude(), + prev_coords.get_longitude(), + curr_coords.get_latitude(), + curr_coords.get_longitude(), + ) + true_bearing: float = res["azi1"] + + logger.debug(f"True bearing: `{true_bearing}`°") + return true_bearing + + @dataclass() class GpsReport: """ A container for GPS data. Sendable across threads. """ - - coordinates: Coordinate = Coordinate(0.0, 0.0) + + # important: ensure you keep these fields matched with GpsController + current_coordinates: Coordinate = Coordinate(0.0, 0.0) height: float = 0.0 # in meters time: float = 0.0 # "time of week" in milliseconds error: float = 0.0 # in millimeters, accounting for vert/horiz error - angle: float = 0.0 # in degrees + true_bearing: float = 0.0 # in degrees previous_coordinates: Coordinate = Coordinate(0.0, 0.0) \ No newline at end of file From 4312844ea41d17f8ca9172a650a966a22625e119 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:23:59 -0500 Subject: [PATCH 17/43] fix(comm.py): use Socket.extend list syntax Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/communication.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/libs/communication.py b/libs/communication.py index 8a428f5..6c1ef6f 100644 --- a/libs/communication.py +++ b/libs/communication.py @@ -58,7 +58,7 @@ def send_wheel_speeds( message: bytearray = bytearray(9) # the wheels use 9 bytes # add subsystem and part bytes - message.extend(self.WHEEL_SUBSYSTEM_BYTE, self.WHEEL_PART_BYTE) + message.extend([self.WHEEL_SUBSYSTEM_BYTE, self.WHEEL_PART_BYTE]) # stick in the speeds message.extend( @@ -67,14 +67,13 @@ def send_wheel_speeds( # compute + add checksum checksum = self.__checksum(message) - message.extend(checksum) + message.extend([checksum]) # send the message over udp 🥺 # TODO: consider using Google's QUIC instead! self.socket.sendall(message) - logger.debug(f"Sending wheel speeds: {self.__prettyprint_byte_array(message)}") + logger.info(f"Sending wheel speeds: {self.__prettyprint_byte_array(message)}") - # Send LED Command def send_led_command(self, red: int, green: int, blue: int): """ Sends the given LED color to the rover. All colors should be in the @@ -83,8 +82,8 @@ def send_led_command(self, red: int, green: int, blue: int): These are not checksummed. """ message: bytearray = bytearray(5) # LEDs use 5 bytes - message.extend(self.LED_SUBSYSTEM_BYTE, self.LED_PART_BYTE) - message.extend(red, green, blue) + message.extend([self.LED_SUBSYSTEM_BYTE, self.LED_PART_BYTE]) + message.extend([red, green, blue]) self.socket.sendall(message) pass @@ -118,4 +117,4 @@ def __prettyprint_byte_array(self, byte_array: bytearray) -> str: if index != len(byte_array) - 1: pretty_print += ", " pretty_print += "]" - print(pretty_print) + return pretty_print From ea03e2cec0db4b2766ca55566ca6f02427ac1e4a Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:24:29 -0500 Subject: [PATCH 18/43] feat(comm.py): no longer use -90 to 90 speeds This changes the speeds to use the 0-255 syntax, where 126 is the neutral/stopped speed. Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/communication.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/libs/communication.py b/libs/communication.py index 6c1ef6f..25e0181 100644 --- a/libs/communication.py +++ b/libs/communication.py @@ -30,16 +30,10 @@ def send_simple_wheel_speeds(self, left_speed: int, right_speed: int): side uses the same speed for all their wheels. """ - def fix(speed: int) -> int: - # TODO: sanity check - fixed_speed = int((speed / 91.0) * 126) + (ls, rs) = (left_speed, right_speed) + if ls < 0 or ls > 255 or rs < 0 or rs > 255: + logger.error("HEY, speeds are wrong! `{ls}` and `{rs}` must be in [0, 255]") - if fixed_speed < 0 or fixed_speed > 255: - logger.error("HEY, fixed speed is wrong! `{speed}` must be in [0, 255]") - - return fixed_speed - - (ls, rs) = (fix(left_speed), fix(right_speed)) self.send_wheel_speeds(ls, ls, ls, rs, rs, rs) def send_wheel_speeds( From 53b0c3d743a35ab67ec70c426fda5a1e26d421d9 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:24:50 -0500 Subject: [PATCH 19/43] feat(Comm): add `send_lights_off` method Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/communication.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libs/communication.py b/libs/communication.py index 25e0181..66ea52a 100644 --- a/libs/communication.py +++ b/libs/communication.py @@ -82,6 +82,10 @@ def send_led_command(self, red: int, green: int, blue: int): pass + def send_lights_off(self): + """Turns off the lights.""" + self.send_led_command(0, 0, 0) + def send_led_red(self): """Makes the LEDs red.""" self.send_led_command(255, 0, 0) From 9c7ccd3798c27bc6a9a66c633bbe5f2533669c90 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:25:10 -0500 Subject: [PATCH 20/43] chore: python version for rye Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- .python-version | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.python-version b/.python-version index 9ad6380..ac037f6 100644 --- a/.python-version +++ b/.python-version @@ -1 +1 @@ -3.8.18 +pypy@3.7.13 From 316eb3127a94294af7c9910c9df7aeaf32037f4a Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:26:36 -0500 Subject: [PATCH 21/43] feat(Coordinate): add `get_` to getter methods This fixes the dumb Python runtime errors from attempting to cast the `longitude` field into a function pointer... Why??? Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/navigation.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libs/navigation.py b/libs/navigation.py index 7fd9311..cc6aa55 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -12,10 +12,10 @@ class Coordinate: latitude: float longitude: float - def latitude(self) -> float: + def get_latitude(self) -> float: self.latitude - def longitude(self) -> float: + def get_longitude(self) -> float: self.longitude def set_latitude(self, latitude: float): @@ -28,7 +28,7 @@ def __str__(self): return f"({self.latitude}, {self.longitude})" -@dataclass(kw_only=True) +@dataclass() class Navigation: """ Keeps track of latititude and longitude, distance to objective, and angle to objective. From d00dfd8bad1d88cf8eea985b17c8db09d4893339 Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:27:16 -0500 Subject: [PATCH 22/43] fix(Navigation): Python 3.7 errors Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/navigation.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/libs/navigation.py b/libs/navigation.py index cc6aa55..024b7da 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -31,10 +31,10 @@ def __str__(self): @dataclass() class Navigation: """ - Keeps track of latititude and longitude, distance to objective, and angle to objective. + Keeps track of latititude and longitude of objective, distance to objective, and angle to objective. It also calculates some of these values. - + The rover should run the `finish` method when it is done navigating. - `given_coords`: a given gps coordinate to navigate to. Depending on @@ -43,7 +43,7 @@ class Navigation: config: Config given_coords: Coordinate - gps: GpsController | None = None + gps: Union[GpsController, None] = None def __init__( self, config: Config, given_coords: Coordinate, swift_ip: str, swift_port: int @@ -59,12 +59,13 @@ def __init__( def finish(self): """ Stops the GPS thread and shuts down the GPS controller. - + This should always be called when the rover finishes navigation. """ - self.gps.stop() - self.gps = None - + if self.gps is not None: + self.gps.stop() + self.gps = None + def distance_to_object(self, coord: Coordinate) -> float: """ Finds the distance between the rover and `coord`. @@ -100,8 +101,8 @@ def coordinates_to_object(self, distance_km: float, angle_deg: float) -> Coordin res = earth.Direct( distance_km, angle_deg, - self.rover_coords.latitude(), - self.rover_coords.longitude(), + rover_coords.get_latitude(), + rover_coords.get_longitude(), ) c = Coordinate(res["lat2"], res["lon2"]) @@ -118,7 +119,7 @@ def calculate_angle(self, co1: Coordinate, co2: Coordinate) -> float: """ earth: Geodesic = Geodesic.WGS84 res = earth.Inverse( - co1.latitude(), co1.longitude(), co2.latitude(), co2.longitude() + co1.get_latitude(), co1.get_longitude(), co2.get_latitude(), co2.get_longitude() ) angle: float = res["azi1"] From b9e46bef4797a4645071340bb484582e5dc8f2fa Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:27:51 -0500 Subject: [PATCH 23/43] fix(Navigation): incorrect bearing methods Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/navigation.py | 59 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 45 insertions(+), 14 deletions(-) diff --git a/libs/navigation.py b/libs/navigation.py index 024b7da..a9527f0 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -1,10 +1,13 @@ from dataclasses import dataclass +from typing import Union from geographiclib.geodesic import Geodesic from loguru import logger +from libs import rover from libs.config import Config from libs.gps_controller import GpsController +from gps_controller import calculate_true_bearing @dataclass() @@ -70,25 +73,43 @@ def distance_to_object(self, coord: Coordinate) -> float: """ Finds the distance between the rover and `coord`. """ - earth: Geodesic = Geodesic.WGS84 - res = earth.Inverse( - self.rover_coords.latitude(), - self.rover_coords.longitude(), - coord.latitude(), - coord.longitude(), - ) - distance: float = res["s12"] - - logger.debug( - f"Distance between `{self.rover_coords}` and `{coord}`: `{distance}`m" - ) + if self.gps is None: + logger.error("GPS should be initialized before finding distance.") + return 0.0 + else: + rover_coords = self.gps.coordinates + earth: Geodesic = Geodesic.WGS84 + res = earth.Inverse( + rover_coords.get_latitude(), + rover_coords.get_longitude(), + coord.get_latitude(), + coord.get_longitude(), + ) + distance: float = res["s12"] + + logger.debug(f"Distance between `{rover_coords}` and `{coord}`: `{distance}`m") return distance def angle_to_object(self, coord: Coordinate) -> float: """ Finds the angle between the rover and `coord`. """ - return calculate_angle(self.rover_coords, coord) + + # Get all the cool stuff to calculate bearing from rover to coordinate + rover_coords = self.gps.coordinates + rover_true_bearing = self.gps.true_bearing + true_bearing_to_coordinate = calculate_true_bearing(rover_coords, coord) + + # The relative bearing to an coordinate is the difference between + # the true bearing of the coordinate and the true bearing of the rover + relative_bearing = true_bearing_to_coordinate - rover_true_bearing + + # Ensure relative bearing is > -180 and < 180 + if relative_bearing < -180: # Bearing is < 180 so add 360 + return relative_bearing + 360 + elif relative_bearing > 180: # Bearing is > 180 so subtract 360 + return relative_bearing - 360 + return relative_bearing # The bearing is just right :3 def coordinates_to_object(self, distance_km: float, angle_deg: float) -> Coordinate: """ @@ -97,6 +118,8 @@ def coordinates_to_object(self, distance_km: float, angle_deg: float) -> Coordin - `distance_km`: The object's distance from the Rover in kilometers. - `angle_deg`: The object's angle from the Rover in degrees. """ + rover_coords = self.__get_gps_coordinates() + earth: Geodesic = Geodesic.WGS84 res = earth.Direct( distance_km, @@ -109,10 +132,18 @@ def coordinates_to_object(self, distance_km: float, angle_deg: float) -> Coordin logger.debug(f"Object coordinates (lat, lon): {c}") return c + def __get_gps_coordinates(self) -> Coordinate: + if self.gps is None: + logger.error("GPS was expected to be initialized, but it wasn't!") + return Coordinate(0.0, 0.0) + else: + return self.gps.coordinates + def calculate_angle(self, co1: Coordinate, co2: Coordinate) -> float: """ Calculates the angle between two coordinates. + The angle between the direction of the rover and North - `co1`: first coordinate. - `co2`: second coordinate. @@ -124,4 +155,4 @@ def calculate_angle(self, co1: Coordinate, co2: Coordinate) -> float: angle: float = res["azi1"] logger.debug(f"Angle between `{co1}` and `{co2}`: `{angle}`°") - return angle \ No newline at end of file + return angle From 756d7ab2ca9259c54fbb0618d2b3fd53ab381daf Mon Sep 17 00:00:00 2001 From: barrett Date: Sat, 25 May 2024 13:28:12 -0500 Subject: [PATCH 24/43] fix(gps_controller.py): formatting Co-Authored-By: Tyler Roman <92000197+ROMANT21@users.noreply.github.com> --- libs/gps_controller.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libs/gps_controller.py b/libs/gps_controller.py index c427429..fcc40f1 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -74,7 +74,7 @@ def check(self): self.true_bearing = calculate_true_bearing( self.previous_coordinates, self.coordinates ) - + # TODO: make report and send to navigation thread sleep(self.SLEEP_TIME) @@ -117,4 +117,3 @@ class GpsReport: error: float = 0.0 # in millimeters, accounting for vert/horiz error true_bearing: float = 0.0 # in degrees previous_coordinates: Coordinate = Coordinate(0.0, 0.0) - \ No newline at end of file From cfd48e06895eb5c4208f1e910b7ada94a9e32647 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 17:16:13 -0500 Subject: [PATCH 25/43] feat(tools/list_cameras.py): add opencv camera list tool --- tools/list_cameras.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 tools/list_cameras.py diff --git a/tools/list_cameras.py b/tools/list_cameras.py new file mode 100644 index 0000000..badeba6 --- /dev/null +++ b/tools/list_cameras.py @@ -0,0 +1,12 @@ +from typing import List + +from cv2 import VideoCapture +from loguru import logger + +for i in range(0, 10): + try: + v: VideoCapture = VideoCapture(index=i) + if v.isOpened(): + logger.info(f"Found a camera! Camera {{ id: {i} }}") + except Exception: + pass \ No newline at end of file From bb541d4b7b91b2990e8f26880a099d26de81fa91 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 18:29:04 -0500 Subject: [PATCH 26/43] feat(Config): let users pass in a config path --- libs/config.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libs/config.py b/libs/config.py index 3c3608e..4dae325 100644 --- a/libs/config.py +++ b/libs/config.py @@ -1,4 +1,5 @@ from configparser import ConfigParser +from loguru import logger from dataclasses import dataclass @@ -10,6 +11,13 @@ class Config: """ config: ConfigParser + + def __init__(self, config_path: str): + c = ConfigParser() + if not c.read(config_path): + logger.error("Failed to read the config from given config path: `{config_path}`") + + self.config = c ##### MBED, for controlling the LEDs ##### From c9de80b3b80a7eb82dc013a4a1f45c413ec3a4ae Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 18:33:11 -0500 Subject: [PATCH 27/43] feat(Communication): use `Config` to get IP/port --- libs/communication.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libs/communication.py b/libs/communication.py index 66ea52a..08dbaa1 100644 --- a/libs/communication.py +++ b/libs/communication.py @@ -3,6 +3,8 @@ from loguru import logger +from libs.config import Config + @dataclass() class Communication: @@ -20,9 +22,9 @@ class Communication: LED_SUBSYSTEM_BYTE = 0x01 LED_PART_BYTE = 0x02 - def __init__(self, rover_ip: str, rover_port: int): + def __init__(self, c: Config): self.socket = Socket(Socket.AF_INET, Socket.SOCK_DGRAM) - self.socket.connect((rover_ip, rover_port)) # adds ip/port to socket's state + self.socket.connect((c.mbed_ip, c.mbed_port)) # adds ip/port to socket's state def send_simple_wheel_speeds(self, left_speed: int, right_speed: int): """ From 1276e29cfc28cfe712cbb9f65880ff1220792328 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 18:45:45 -0500 Subject: [PATCH 28/43] feat(Communiction): return the sent bytearrays --- libs/communication.py | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/libs/communication.py b/libs/communication.py index 08dbaa1..db5050a 100644 --- a/libs/communication.py +++ b/libs/communication.py @@ -26,7 +26,7 @@ def __init__(self, c: Config): self.socket = Socket(Socket.AF_INET, Socket.SOCK_DGRAM) self.socket.connect((c.mbed_ip, c.mbed_port)) # adds ip/port to socket's state - def send_simple_wheel_speeds(self, left_speed: int, right_speed: int): + def send_simple_wheel_speeds(self, left_speed: int, right_speed: int) -> bytearray: """ Sends the given wheel side speeds to the rover. In this case, each side uses the same speed for all their wheels. @@ -36,7 +36,7 @@ def send_simple_wheel_speeds(self, left_speed: int, right_speed: int): if ls < 0 or ls > 255 or rs < 0 or rs > 255: logger.error("HEY, speeds are wrong! `{ls}` and `{rs}` must be in [0, 255]") - self.send_wheel_speeds(ls, ls, ls, rs, rs, rs) + return self.send_wheel_speeds(ls, ls, ls, rs, rs, rs) def send_wheel_speeds( self, @@ -46,7 +46,7 @@ def send_wheel_speeds( front_right: int, middle_right: int, rear_right: int, - ): + ) -> bytearray: """ Sends the given wheel speeds to the rover. Each wheel speed goes from 0 to 255 (u8::MAX), with 126 being neutral. @@ -69,8 +69,9 @@ def send_wheel_speeds( # TODO: consider using Google's QUIC instead! self.socket.sendall(message) logger.info(f"Sending wheel speeds: {self.__prettyprint_byte_array(message)}") + return message - def send_led_command(self, red: int, green: int, blue: int): + def send_led_command(self, red: int, green: int, blue: int) -> bytearray: """ Sends the given LED color to the rover. All colors should be in the range of [0, 255]. @@ -81,24 +82,25 @@ def send_led_command(self, red: int, green: int, blue: int): message.extend([self.LED_SUBSYSTEM_BYTE, self.LED_PART_BYTE]) message.extend([red, green, blue]) self.socket.sendall(message) + + logger.info(f"Sending command to LEDs: `{self.__prettyprint_byte_array()}`") + return message - pass - - def send_lights_off(self): + def send_lights_off(self) -> bytearray: """Turns off the lights.""" - self.send_led_command(0, 0, 0) + return self.send_led_command(0, 0, 0) - def send_led_red(self): + def send_led_red(self) -> bytearray: """Makes the LEDs red.""" - self.send_led_command(255, 0, 0) + return self.send_led_command(255, 0, 0) - def send_led_green(self): + def send_led_green(self) -> bytearray: """Makes the LEDs green.""" - self.send_led_command(0, 255, 0) + return self.send_led_command(0, 255, 0) - def send_led_blue(self): + def send_led_blue(self) -> bytearray: """Makes the LEDs blue.""" - self.send_led_command(0, 0, 255) + return self.send_led_command(0, 0, 255) def __checksum(self, byte_array: bytearray) -> int: """ From 67eba7a4688b635e45b507afc65156ddc246df79 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 19:20:01 -0500 Subject: [PATCH 29/43] refactor(GpsController): use a Process --- libs/gps_controller.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/libs/gps_controller.py b/libs/gps_controller.py index fcc40f1..384ff55 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from threading import Thread +from multiprocessing import Process from time import sleep from loguru import logger @@ -26,6 +26,9 @@ class GpsController: error: float = 0.0 # in millimeters, accounting for vert/horiz error true_bearing: float = 0.0 # Bearing from True North previous_coordinates: Coordinate = Coordinate(0.0, 0.0) + + # here's the process we can join on when the gps is closed + process: Process SLEEP_TIME: float = 0.1 @@ -37,15 +40,17 @@ def __init__(self, swift_ip: str, swift_port: int): def start(self): """ - Starts the GPS thread and connects to the GPS hardware itself. + Starts the GPS process and connects to the GPS hardware itself. """ gps.gps_init(self.conf.gps_ip(), self.conf.gps_port()) self.enabled = True - logger.info("Starting GPS thread...") - t = Thread(target=self.check, name=("GPS thread"), args=()) - t.start() - logger.info("GPS thread started!") + logger.info("Starting GPS process...") + proc = Process(target=self.check, name="GPS Process") + proc.start() + logger.info("GPS process started!") + + self.process = proc pass def stop(self): From 4541a979309f510c9b60f756038a6b8a835a937c Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 19:20:17 -0500 Subject: [PATCH 30/43] fix(GpsController): join the process back --- libs/gps_controller.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libs/gps_controller.py b/libs/gps_controller.py index 384ff55..5a33cd8 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -59,6 +59,9 @@ def stop(self): """ self.enabled = False gps.gps_finish() + logger.info("Joining the GPS Controller process...") + self.process.close() + logger.info("GPS Controller process joined!") def check(self): while self.enabled: From 18d66f465ef3abab96b11bd7e473bbd377576e10 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 19:20:43 -0500 Subject: [PATCH 31/43] feat(GpsController): use a Config input for constructor --- libs/gps_controller.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libs/gps_controller.py b/libs/gps_controller.py index 5a33cd8..b364c7d 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -32,7 +32,10 @@ class GpsController: SLEEP_TIME: float = 0.1 - def __init__(self, swift_ip: str, swift_port: int): + def __init__(self, c: config.Config): + self.conf = c + + # TODO? """ Initialize Location object to get coordinates """ From 2d60cf5f84f8f4e1addb76dc9200b5a016ca284d Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 19:21:15 -0500 Subject: [PATCH 32/43] feat(args.py): Initial version --- libs/args.py | 93 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 libs/args.py diff --git a/libs/args.py b/libs/args.py new file mode 100644 index 0000000..79dfa22 --- /dev/null +++ b/libs/args.py @@ -0,0 +1,93 @@ +from argparse import ArgumentParser, Namespace +from dataclasses import dataclass +from typing import Union, List +from loguru import logger + +from libs.navigation import Coordinate + + +@dataclass +class Args: + aruco_id: Union[int, None] # the ArUco tag ID to search for, if any + coord_file_path: str # a file that's in the format: `lat lon`, each on a new line + coordinate_list: List[Coordinate] + camera_id: int # the camera to use. defaults to 0 + + def __init__(self): + """ + Gets the command line arugments and updates internal state. + """ + arg_parser = ArgumentParser() + + # get opencv camera id + arg_parser.add_argument( + "--camera_id", + "-c", + required=False, + default=0, + type=int, + help="The OpenCV capture device (camera) ID to use. Use `tools/camera_list.py` to determine usable values.", + ) + + # get the coordinate file path + arg_parser.add_argument( + "--coords", + "-f", + required=True, + type=str, + help="A file path pointing to a list of coordinates. The file should have one coordinate pair per line, in this format: `lat long`", + ) + + # get the aruco ID to use + arg_parser.add_argument( + "--aruco_id", + "-a", + required=False, + type=int, + help="The ArUco ID number to use from the DICT_4X4_50 list. Safe testing values are 0 and 1.", + ) + + # place all data into internal state + args: Namespace = arg_parser.parse_args() + + self.aruco_id = args.aruco_id + if type(self.aruco_id) is not None and ( + self.aruco_id > 50 or self.aruco_id < 0 + ): + logger.error( + f"You must pass an ArUco ID within the range: [0, 50]. You gave: `{self.aruco_id}`." + ) + exit(1) + + self.camera_id = args.camera_id if args.camera_id else 0 + + self.coord_file_path = args.coords + coordinate_list = [] + + with open(self.coord_file_path) as file: + for line in file: + try: + (lat_str, lon_str) = line.split(" ") + + # attempt to parse the strings into two numbers + (lat, lon) = ( + float(lat_str.replace("\ufeff", "")), + float(lon_str.replace("\ufeff", "")), + ) + + # create a `Coordinate` from the data + coordinate_list.append(Coordinate(lat, lon)) + except ValueError as e: + logger.error( + f"Failed to parse given coordinate file. Failed with error: `{e}`" + ) + exit(1) + + file.close() + self.coordinate_list = coordinate_list + logger.info(f"The coordinates file was parsed successfully! Here's the list: {self.coordinate_list}") + + + pass + + pass From 3d3ecaa17a0f95ea414fe829b28d518a0234f9ff Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 19:54:16 -0500 Subject: [PATCH 33/43] chore: Remove unused libs/Locationf9p.py --- libs/Locationf9p.py | 171 -------------------------------------------- 1 file changed, 171 deletions(-) delete mode 100755 libs/Locationf9p.py diff --git a/libs/Locationf9p.py b/libs/Locationf9p.py deleted file mode 100755 index a32ba25..0000000 --- a/libs/Locationf9p.py +++ /dev/null @@ -1,171 +0,0 @@ -import sys - -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 -# heading and see if that should be computed -# somewhere outside of __parse -class LocationF9P: - # To find the device path, run ls -l /dev/serial/by-id/usb-ublox_....(tab complete this) - # and it will return where it's symlinked to, most likely /dev/ttyACM0 - # you could probably just use that /dev/serial path too - def __init__(self, device_path="/dev/ttyACM0"): - self.device_path = device_path - self.device_open_file = None - self.latitude = 0 - self.longitude = 0 - self.old_latitude = 0 - self.old_longitude = 0 - self.bearing = 0.0 - self.running = True - - def config(self): - # read from a file, probably configure this to work with - # ConfigParser because that's ez - pass - - # Returns distance in kilometers between given latitude and longitude - 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) - - 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): - 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 is None: - 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.daemon = True - t.start() - - # Stops updating fields from GPS box - def stop_GPS_thread(self): - self.running = False - - def start_GPS(self): - self.device_open_file = open(self.device_path) - - def update_fields_loop(self): - while self.running: - 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. - # The protocol is described on pdf page 24 - # of the integration manual (section 4.2.6) - # https://cdn.sparkfun.com/assets/f/7/4/3/5/PM-15136.pdf - def __parse(self, message_str): - if isinstance(message_str, str): - split = message_str.split(",") - # look for lat/lon messages. We'll almost certainly get - # GN messages (maybe only GN messages, but this theoretically accepts other talkers) - if split[0].startswith("$") and split[0].endswith("GLL"): - computed_checksum = 0 - for char in message_str: - if char == "*": - # marks the end of the portion to hash - break - elif char == "$": - # marks the beginning - computed_checksum = 0 - else: - computed_checksum ^= ord(char) - - computed_checksum = format(computed_checksum, "X") - message_checksum = ( - message_str.split("*")[-1].replace("\n", "").replace("\r", "") - ) - if computed_checksum != message_checksum: - print( - "`" - + format(computed_checksum, "X") - + "` did not match `" - + message_str.split("*")[-1] - + "`" - ) - return - - self.old_latitude = self.latitude - self.old_longitude = self.longitude - - # This is just a big pile of slicing up, the goal is to take: - # $GNGLL,3511.93307,N,09721.15557,W,011244.00,A,D*64 - # and output the lat and lon. - # It does this by: - # - Parsing out the integer number of degrees (2 digits for lat, 3 digits for lon) - # - Parsing out the minutes, and dividing them by 60, then adding them to the integer degrees - # - Multiplying by -1 if the lat is in the south, or if the lon is in the west - self.latitude = (int(split[1][0:2]) + float(split[1][2:]) / 60) * ( - -1 if split[2] == "S" else 1 - ) - self.longitude = (int(split[3][0:3]) + float(split[3][3:]) / 60) * ( - -1 if split[4] == "W" else 1 - ) - - 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("$"): - print( - "got weird message: " + message_str + " of len " + str(len(split)) - ) - - # 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) - - # Calculate latitutde and longitude given distance (in km) and bearing (in degrees) - 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 - d = distance - - 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) - ) - - return degrees(lat2), degrees(lon2) From af1c53b35331e4c98427cb4528bf8ec5be27fcc9 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:04:26 -0500 Subject: [PATCH 34/43] chore(list_cameras.py): remove unused import --- tools/list_cameras.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/tools/list_cameras.py b/tools/list_cameras.py index badeba6..37d1e80 100644 --- a/tools/list_cameras.py +++ b/tools/list_cameras.py @@ -1,5 +1,3 @@ -from typing import List - from cv2 import VideoCapture from loguru import logger @@ -9,4 +7,4 @@ if v.isOpened(): logger.info(f"Found a camera! Camera {{ id: {i} }}") except Exception: - pass \ No newline at end of file + pass From 3c5d8b4d3e8af8dc7b137cdfd4ef2d74a63ae459 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:49:24 -0500 Subject: [PATCH 35/43] feat(Coordinate): move to its own module --- libs/args.py | 2 +- libs/coordinate.py | 21 +++++++++++++++++++++ libs/gps_controller.py | 2 +- libs/navigation.py | 25 ++----------------------- libs/rover.py | 4 ++-- 5 files changed, 27 insertions(+), 27 deletions(-) create mode 100644 libs/coordinate.py diff --git a/libs/args.py b/libs/args.py index 79dfa22..7326456 100644 --- a/libs/args.py +++ b/libs/args.py @@ -3,7 +3,7 @@ from typing import Union, List from loguru import logger -from libs.navigation import Coordinate +from libs.coordinate import Coordinate @dataclass diff --git a/libs/coordinate.py b/libs/coordinate.py new file mode 100644 index 0000000..3bba311 --- /dev/null +++ b/libs/coordinate.py @@ -0,0 +1,21 @@ +from dataclasses import dataclass + +@dataclass +class Coordinate: + latitude: float + longitude: float + + def get_latitude(self) -> float: + self.latitude + + def get_longitude(self) -> float: + self.longitude + + def set_latitude(self, latitude: float): + self.latitude = latitude + + def set_longitude(self, longitude: float): + self.longitude = longitude + + def __str__(self): + return f"({self.latitude}, {self.longitude})" \ No newline at end of file diff --git a/libs/gps_controller.py b/libs/gps_controller.py index b364c7d..db7e20f 100644 --- a/libs/gps_controller.py +++ b/libs/gps_controller.py @@ -7,7 +7,7 @@ from gps import gps from libs import config -from libs.navigation import Coordinate +from libs.coordinate import Coordinate @dataclass() diff --git a/libs/navigation.py b/libs/navigation.py index a9527f0..862fa80 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -6,29 +6,8 @@ from libs import rover from libs.config import Config -from libs.gps_controller import GpsController -from gps_controller import calculate_true_bearing - - -@dataclass() -class Coordinate: - latitude: float - longitude: float - - def get_latitude(self) -> float: - self.latitude - - def get_longitude(self) -> float: - self.longitude - - def set_latitude(self, latitude: float): - self.latitude = latitude - - def set_longitude(self, longitude: float): - self.longitude = longitude - - def __str__(self): - return f"({self.latitude}, {self.longitude})" +from libs.coordinate import Coordinate +from libs.gps_controller import GpsController, calculate_true_bearing, GpsReport @dataclass() diff --git a/libs/rover.py b/libs/rover.py index 8d84e04..4038bb0 100644 --- a/libs/rover.py +++ b/libs/rover.py @@ -8,8 +8,8 @@ from libs import config from libs.communication import Communication -from libs.navigation import Coordinate, Navigation -from maps import MapServer +from libs.navigation import Navigation +from maps.server import MapServer @dataclass From 5c8c765f1d9e542d4dbf23eed19f375e39cc7de4 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:49:55 -0500 Subject: [PATCH 36/43] fix(Args): check if id is None first --- libs/args.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/libs/args.py b/libs/args.py index 7326456..018d02b 100644 --- a/libs/args.py +++ b/libs/args.py @@ -51,13 +51,12 @@ def __init__(self): args: Namespace = arg_parser.parse_args() self.aruco_id = args.aruco_id - if type(self.aruco_id) is not None and ( - self.aruco_id > 50 or self.aruco_id < 0 - ): - logger.error( - f"You must pass an ArUco ID within the range: [0, 50]. You gave: `{self.aruco_id}`." - ) - exit(1) + if self.aruco_id is not None: + if self.aruco_id > 50 or self.aruco_id < 0: + logger.error( + f"You must pass an ArUco ID within the range: [0, 50]. You gave: `{self.aruco_id}`." + ) + exit(1) self.camera_id = args.camera_id if args.camera_id else 0 From 34724a5346fe090a87cd9901a032860eaf6c6d5a Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:50:45 -0500 Subject: [PATCH 37/43] fix(Config): missed f-string lol --- libs/config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/config.py b/libs/config.py index 4dae325..e4da434 100644 --- a/libs/config.py +++ b/libs/config.py @@ -15,7 +15,7 @@ class Config: def __init__(self, config_path: str): c = ConfigParser() if not c.read(config_path): - logger.error("Failed to read the config from given config path: `{config_path}`") + logger.error(f"Failed to read the config from given config path: `{config_path}`") self.config = c From f8ace7849c6285d542fd8b5a7d88d2bfef145151 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:51:44 -0500 Subject: [PATCH 38/43] fix(ArucoTracker): use specific dict for dataclass typing --- libs/aruco_tracker.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/aruco_tracker.py b/libs/aruco_tracker.py index 878fea2..752f043 100755 --- a/libs/aruco_tracker.py +++ b/libs/aruco_tracker.py @@ -16,7 +16,7 @@ class ArucoTracker: # TODO: Find out how to get camera matrix and distance coefficients camera_mtx: type # TODO distance_coefficients: type # TODO - aruco_dictionary: cv2.aruco.Dictionary + aruco_dictionary: cv2.aruco.DICT_4X4_50 video_capture: cv2.VideoCapture def __init__(self, opencv_camera_index: int): From 335bf3ea0172f9c0a90c6e479b4ddeebc61c7cd9 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:52:27 -0500 Subject: [PATCH 39/43] feat: Add `new_main.py` --- new_main.py | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 new_main.py diff --git a/new_main.py b/new_main.py new file mode 100644 index 0000000..cab32aa --- /dev/null +++ b/new_main.py @@ -0,0 +1,10 @@ +from loguru import logger +from libs.rover import Rover + +logger.info("Starting Autonomous...") +remi = Rover() + +# this generally shouldn't happen unless the Rover first specifies +logger.warning("Autonomous code ended. Main thread exiting...") +logger.warning("Make sure to use Ctrl^C if other threads continue to run.") +exit(0) \ No newline at end of file From 245574fa2adccdb942dffdb12c225d2ded57b700 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:53:49 -0500 Subject: [PATCH 40/43] feat(Rover): get config from `config.py` --- libs/rover.py | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/libs/rover.py b/libs/rover.py index 4038bb0..5733da8 100644 --- a/libs/rover.py +++ b/libs/rover.py @@ -26,8 +26,10 @@ class Rover: """ A representation of the rover. """ - - conf: config.Config + arguments: Args + conf: Config + mode: Mode + rover_connection: Communication opencv_camera_index: int nav: Navigation @@ -35,18 +37,8 @@ class Rover: def __init__(self, given_coords: Coordinate, opencv_camera_index: int): # sets up the parser - config = configparser.ConfigParser(allow_no_value=True) - config.read(os.path.dirname(__file__) + "/../config.ini") - - # initialize the gps object - # this starts a thread for the gps - self.nav = Navigation( - config, - given_coords, - swift_ip=self.conf.gps_ip(), - swift_port=self.conf.gps_port(), - ) - self.gps.start_GPS() + self.conf = Config("../config.ini") + self.speeds = [0, 0] self.error_accumulation = 0.0 From e5320d38321315416d471cd4e4bd385fcf798acb Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:54:32 -0500 Subject: [PATCH 41/43] feat(Rover): more realistic layout --- libs/rover.py | 52 ++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 41 insertions(+), 11 deletions(-) diff --git a/libs/rover.py b/libs/rover.py index 5733da8..bd00b19 100644 --- a/libs/rover.py +++ b/libs/rover.py @@ -31,11 +31,34 @@ class Rover: mode: Mode rover_connection: Communication - opencv_camera_index: int + communication_queue: Queue + communication_process: Process + nav: Navigation - mode: Mode + navigation_queue: Queue + + aruco_tracker: ArucoTracker + aruco_tracker_queue: Queue - def __init__(self, given_coords: Coordinate, opencv_camera_index: int): + def __init__(self): + # get arguments + logger.info("Parsing arguments...") + self.arguments = Args() + + self.mode = Mode.SEARCHING_ALONG_COORDINATE + + # check arguments for aruco id. set self.mode accordingly + if self.arguments.aruco_id is None: + logger.info("No ArUco markers were given. Let's navigate to the given coordinates.") + self.mode = self.mode + # go to coordinate + + else: + self.mode = Mode.SEARCHING_ALONG_COORDINATE + # go to coordinate + # TODO: when that's done, look for the marker + logger.info("Parsed arguments successfully!") + # sets up the parser self.conf = Config("../config.ini") @@ -49,21 +72,28 @@ def __init__(self, given_coords: Coordinate, opencv_camera_index: int): # - Aruco Tracker/YOLO thread # - RoverMap Thread - self.opencv_camera_index = opencv_camera_index self.rover_connection = Communication(self.conf.gps_ip(), self.conf.gps_port()) pass - def start_navigation_thread(self): - # What does navigati + def start_navigation(self): + # initialize the gps object + # this starts a thread for the gps, as this module handles that itself + self.nav = Navigation( + self.conf, + self.arguments.coordinate_list, # FIXME: needs to take a list??? or manage it manual + ) + pass - def start_communication_thread(self): + def start_communication(self): # starts the thread that sends wheel speeds - self.running = True - t = Thread(target=self.send_speed, name=("send wheel speeds"), args=()) - t.daemon = True - t.start() + proc = Process(target=self.rover_connection.handle_commands, name="send wheel speeds") # FIXME + proc.daemon = True # FIXME: don't do this + proc.start() + + logger.info("Started the communication thread!") + self.communication_process = proc pass def start_aruco_tracker_thread(self): From 76514a475cfc40f66c23239e35f5b4110a70c8ef Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:55:50 -0500 Subject: [PATCH 42/43] fix(Rover): imports (vscode on drugs?) --- libs/rover.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libs/rover.py b/libs/rover.py index bd00b19..6e2bf88 100644 --- a/libs/rover.py +++ b/libs/rover.py @@ -1,12 +1,13 @@ -import configparser -import os from dataclasses import dataclass -from threading import Thread +from multiprocessing import Process, Queue from time import sleep import cv2 +from loguru import logger -from libs import config +from libs.config import Config +from libs.args import Args +from libs.aruco_tracker import ArucoTracker from libs.communication import Communication from libs.navigation import Navigation from maps.server import MapServer From 898bac9d665d774e2fa4c27bd2c9feacf5130bc3 Mon Sep 17 00:00:00 2001 From: REMI Date: Sat, 25 May 2024 20:58:31 -0500 Subject: [PATCH 43/43] fix(Navigation): config, queue, navigate --- libs/navigation.py | 53 ++++++++++++++++++++++++++++++---------------- 1 file changed, 35 insertions(+), 18 deletions(-) diff --git a/libs/navigation.py b/libs/navigation.py index 862fa80..36180ab 100644 --- a/libs/navigation.py +++ b/libs/navigation.py @@ -1,10 +1,9 @@ from dataclasses import dataclass -from typing import Union +from multiprocessing import Queue from geographiclib.geodesic import Geodesic from loguru import logger -from libs import rover from libs.config import Config from libs.coordinate import Coordinate from libs.gps_controller import GpsController, calculate_true_bearing, GpsReport @@ -23,19 +22,21 @@ class Navigation: section of comp, ARUCO tags will be nearby """ - config: Config + conf: Config given_coords: Coordinate - gps: Union[GpsController, None] = None + gps: GpsController + + queue: Queue def __init__( - self, config: Config, given_coords: Coordinate, swift_ip: str, swift_port: int + self, conf: Config, given_coords: Coordinate ): """ Initializes the navigation to get coordinates. """ self.given_coords = given_coords - self.config = config - self.gps = GpsController(swift_ip, swift_port) + self.conf = conf + self.gps = GpsController(self.conf) self.gps.start() def finish(self): @@ -44,9 +45,8 @@ def finish(self): This should always be called when the rover finishes navigation. """ - if self.gps is not None: - self.gps.stop() - self.gps = None + self.gps.stop() + self.gps = None def distance_to_object(self, coord: Coordinate) -> float: """ @@ -97,7 +97,7 @@ def coordinates_to_object(self, distance_km: float, angle_deg: float) -> Coordin - `distance_km`: The object's distance from the Rover in kilometers. - `angle_deg`: The object's angle from the Rover in degrees. """ - rover_coords = self.__get_gps_coordinates() + rover_coords = self.gps.coordinates earth: Geodesic = Geodesic.WGS84 res = earth.Direct( @@ -110,13 +110,30 @@ def coordinates_to_object(self, distance_km: float, angle_deg: float) -> Coordin logger.debug(f"Object coordinates (lat, lon): {c}") return c - - def __get_gps_coordinates(self) -> Coordinate: - if self.gps is None: - logger.error("GPS was expected to be initialized, but it wasn't!") - return Coordinate(0.0, 0.0) - else: - return self.gps.coordinates + + def navigate(self): + """ + A method that runs until `not self.enabled`. Checks for a new + GpsReport. + + If one is present, does logic based on the rover's mode and location. + """ + while self.enabled: + # check for new gps report + if not self.queue.empty(): + message = self.queue.get() + + if type(message) is GpsReport: + logger.debug("Found new GPS report! Let's operate on it...") + self.drive(message) + + + def drive(self, message: GpsReport): + # UNIMPLEMENTED! + pass + + + def calculate_angle(self, co1: Coordinate, co2: Coordinate) -> float: