diff --git a/robot/__init__.py b/robot/__init__.py index 0782bb7..cf913f6 100644 --- a/robot/__init__.py +++ b/robot/__init__.py @@ -25,7 +25,7 @@ from robot.wrapper import Robot, NoCameraPresent from robot.greengiant import OUTPUT, INPUT, INPUT_ANALOG, INPUT_PULLUP, PWM_SERVO from robot.vision import RoboConUSBCamera -from robot.marker_setup import ( +from robot.game_config import ( MARKER, BASE_MARKER, ARENA_MARKER, @@ -34,6 +34,7 @@ TEAM ) + MINIUM_VERSION = (3, 6) if sys.version_info <= MINIUM_VERSION: raise ImportError( diff --git a/robot/apriltags3.py b/robot/apriltags3.py index dcec1f7..5d20c71 100755 --- a/robot/apriltags3.py +++ b/robot/apriltags3.py @@ -21,7 +21,7 @@ import numpy as np import scipy.spatial.transform as transform -from robot.marker_setup.markers import MARKER +from robot.game_config import MARKER ###################################################################### @@ -435,7 +435,7 @@ def detect(self, img, estimate_tag_pose=False, camera_params=None): if camera_params is None: raise ValueError( "camera_params must be provided to detect if estimate_tag_pose is set to True") - tag_size = MARKER.by_id(tag.id).size + tag_size = MARKER(tag.id).size camera_fx, camera_fy, camera_cx, camera_cy = [ c for c in camera_params] diff --git a/robot/calibrate_camera.py b/robot/calibrate_camera.py deleted file mode 100755 index 2d8c31c..0000000 --- a/robot/calibrate_camera.py +++ /dev/null @@ -1,71 +0,0 @@ -"""A script for getting the focal length luts for a camera -It losely follows the ideas of a PD controller combinded with a NM gradient -descent algo. -Usage: -import robot.calibrate_camera -""" -import robot -import math -import pprint - -TARGET = 3.0 -THRESHOLD = 0.01 -KP = 100 -KD = 5 -K_READING_COUNTS = 0.5 - - -def _get_reading(): - while True: - try: - return R.see()[0].dist - except IndexError: - print("saw nothing") - - -def _get_reading_number(error): - result = int(K_READING_COUNTS / error) - result = abs(result) - if result is 0: - result = 1 - elif result > 6: - result = 6 - return result - - -R = robot.Robot() -result = {} - -for res in R.camera.focal_lengths.copy(): - print("Checking res {}".format(res)) - R.camera.res = res - pprint.pprint(R.camera.focal_lengths) - - error = THRESHOLD + 1.0 - previous_error = error - while abs(error) > THRESHOLD: - value = R.camera.focal_lengths[res][0] - p = error * KP - d = (previous_error - error) * KD - value += p + d - - R.camera.focal_lengths[res] = (value, value) - R.camera._update_camera_params() - - reading_counts = get_reading_number(error) - - dists = [get_reading() for _ in range(reading_counts)] - average_dist = (sum(dists))/reading_counts - - previous_error = error - error = TARGET - average_dist - - print("Tried: {} got dist {} error: {}".format( - value, average_dist, error)) - print(" Max: {} min: {} range: {}".format( - max(dists), min(dists), max(dists) - min(dists))) - print(" P = {} reading_counts {}".format(error * KP, reading_counts)) - - result[res] = (value, value) - -pprint.pprint(result) diff --git a/robot/marker_setup/__init__.py b/robot/marker_setup/__init__.py deleted file mode 100644 index 8fa5340..0000000 --- a/robot/marker_setup/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -from .teams import TEAM -from .markers import ( - MARKER, - MARKER_TYPE, - ARENA_MARKER, - POTATO_MARKER, - BASE_MARKER, - POEM_ON_STARTUP, -) - -__all__ = ( - "TEAM", - "MARKER", - "POTATO_MARKER", - "MARKER_TYPE", - "BASE_MARKER", - "ARENA_MARKER", - "POEM_ON_STARTUP", -) diff --git a/robot/marker_setup/markers.py b/robot/marker_setup/markers.py deleted file mode 100644 index 397a10d..0000000 --- a/robot/marker_setup/markers.py +++ /dev/null @@ -1,140 +0,0 @@ -import enum -import typing - -from .teams import TEAM - -""" -Hiiii! -This file contains the definitions for the markers and the data we assign to them. -I have just set them to 2024's competition values, with MARKER_TYPE deciding whether a marker is part of -the ARENA walls, or a game object, which is this year (2023-2024) called a POTATO. So make the changes you -need! But make sure to change every reference to it in the code, not just the ones in this file. -So go nuts! Good luck with your Robocon, and feel free to add your own messages for the future below! -Byee! - - Holly (2023-2024) - -[Put your future messages here] -""" - -class POEM_ON_STARTUP: - jokes = [ - "Why did the potato cross the road? \ - He saw a fork up ahead.", - "What do you say to a baked potato that's angry? \ - Anything you like, just butter it up.", - "Funny Potato Joke", - "Farm Facts: There are around 5000 different varieties of potato", - "Farm Facts: Potatoes were first domesticated in Peru around 4500 years ago around Lake Titicaca", - "Farm Facts: The word potato originates from the Taino \"batata\", meaning sweet potato.", - "Farm Facts: China is the leading producer of potatoes, with 94.3 million tonnes produced in 2021", - "Farm Facts: The maximum theoretical voltage " - ] - - @staticmethod - def on_startup(logger, random): - """ - This is called on startup. Put something funny and relevant to this - years competition using the logger. Also random is currently passed - as an argument because I don't have the energy to try importing it, - I just spent quite a while struggling with the new brains. - """ - jokeNo = random.randint(0,len(POEM_ON_STARTUP.jokes)) - jokeToPrint = "I don't know what went wrong, but we messed up our joke loading ;-;" - try: - jokeToPrint = POEM_ON_STARTUP.jokes[jokeNo] - except: - jokeToPrint = POEM_ON_STARTUP.jokes[0] - logger.info(jokeToPrint) - -class MARKER_TYPE(enum.Enum): # Keep something like this to determine if a marker is a wall or not. - POTATO = enum.auto() - ARENA = enum.auto() - - -class BASE_MARKER: # Base marker class that POTATO_MARKER and ARENA_MARKER derive from. - team_marker_colors: dict = { # Colour definitions for each team defined in TEAMS - TEAM.RUSSET: (255, 64, 0), # RED - TEAM.SWEET: (255, 255, 32), # YELLOW - TEAM.MARIS_PIPER: (50,255,0), # GREEN - TEAM.PURPLE: (255, 32, 255), # PURPLE - } - - def __init__( - self, - id: int, - type: MARKER_TYPE, - ) -> None: - self.id = id - self.type = type - self.owning_team: typing.Union[TEAM, None] = None - - # Sizes are in meters - self.size = 0.2 if self.type == MARKER_TYPE.ARENA else 0.08 - - def __repr__(self) -> str: - return f"" - - @property - def bounding_box_color(self) -> tuple: - if self.type == MARKER_TYPE.ARENA: # If it is a wall - return tuple(reversed((125, 249, 225))) # Turquoise - elif self.owning_team==TEAM.ARENA: # If it is a Hot Potato (game object owned by ARENA) - return tuple(reversed((255,255,255))) # White - else: # If it is a Jacket Potato (game object owned by a team.) - return tuple(reversed(self.team_marker_colors[self.owning_team])) # Picks the team colour from above - -class ARENA_MARKER(BASE_MARKER): # Not much going on here. This represents a wall. - def __init__(self, id: int) -> None: - super().__init__(id, MARKER_TYPE.ARENA) - - def __repr__(self) -> str: - return f"" - - -class POTATO_MARKER(BASE_MARKER): # This is a game object rather than a wall. Add properties you want to keep track of - def __init__( - self, id: int, owner: TEAM - ) -> None: - super().__init__(id, MARKER_TYPE.POTATO) - self.owning_team = owner - - def __repr__(self) -> str: - return f"" - -class MARKER(BASE_MARKER): # This is literally just how the code gets the different marker types. - @staticmethod - def by_id(id: int, team: typing.Union[TEAM, None] = None) -> BASE_MARKER: # team is currently unused, but it is referenced throughout the code. It is the team of the robot I believe (check this) - """ - Get a marker object from an id - - Marker IDs are greater than 0, and usually won't go higher than 200. They are - read as April tags, so use an online 6x6 April tag generator to test values, - such as: - https://chaitanyantr.github.io/apriltag (remember to set to tag36h11 for this one) - - In 2024 low marker IDs (0-39) are potatoes. The first 4 potato markers are Jacket - Potatoes and belong to the team with the corresponding ID. - The rest of the low markers are unowned - their owning_team property is None - and their owner is ARENA. - - It is probably recommendable that you have duplicate marker IDs, so that any damaged - marker can be replaced by one with equivalent game meaning - in 2024 we made ID 0 and - ID 20 equivalent. - - Higher marker IDs (40+) will be part of the arena. These markers will be - spaced as in 2021-2022's competition (6 markers on each side of the Arena, - the first 50cm away from the wall and each subsequent marker 1m away from - there). In practice these markers start at 100. - """ - - ARENA_WALL_LOWER_ID = 40 - if id >= ARENA_WALL_LOWER_ID: - return ARENA_MARKER(id) - - wrappingId = id % 20 # Make sure that the ID range wraps after 20 values. - if wrappingId<4: # If it is a Jacket Potato (has a team) - owning_team = TEAM[f"T{wrappingId}"] # Set to the corresponding TEAM enum. - else: # If it is a Hot Potato (Has no team) - owning_team = TEAM["ARENA"] - - return POTATO_MARKER(id, owning_team) diff --git a/robot/marker_setup/teams.py b/robot/marker_setup/teams.py deleted file mode 100644 index 23e04bf..0000000 --- a/robot/marker_setup/teams.py +++ /dev/null @@ -1,20 +0,0 @@ -import enum - -""" -This defines each team, and a corresponding Tx value for the team (x is the team index). Make sure that -the value of the Tx matches its team, and is unique - this year I picked the latin names for the potato varieties -that the school will be competing as. -""" -class TEAM(enum.Enum): - RUSSET = "Solanum Tuberosum 'Ranger Russet'" # This matches T0, for example. - SWEET = "Ipomoea batatas" - MARIS_PIPER = "Solanum Tuberosum 'Maris Piper'" - PURPLE = "Solanum Tuberosum 'Vitolette'" - ARENA = "HOTTTTT!" - # There is no T value for ARENA, so there is no way that the assignment of team to a marker can accidentally assign ARENA if the logic goes wrong. - - T0 = "Solanum Tuberosum 'Ranger Russet'" - T1 = "Ipomoea batatas" - T2 = "Solanum Tuberosum 'Maris Piper'" - T3 = "Solanum Tuberosum 'Vitolette'" - diff --git a/robot/vision.py b/robot/vision.py index 8c3ab23..8848466 100755 --- a/robot/vision.py +++ b/robot/vision.py @@ -7,11 +7,13 @@ import threading import queue +from collections.abc import Iterable from datetime import datetime from typing import NamedTuple, Any -from robot.marker_setup.markers import MARKER -from .marker_setup import BASE_MARKER as MarkerInfo + +from robot.game_config import MARKER, WHITE +from .game_config import BASE_MARKER as MarkerInfo import cv2 import numpy as np @@ -78,15 +80,6 @@ class Capture(NamedTuple): _USB_IMAGES_PATH = "/media/RobotUSB/collect_images.txt" _USB_LOGS_PATH = "/media/RobotUSB/log_markers.txt" -# Colours are in the format BGR -PURPLE = (255, 0, 215) # Purple -ORANGE = (0, 128, 255) # Orange -YELLOW = (0, 255, 255) # Yellow -GREEN = (0, 255, 0) # Green -RED = (0, 0, 255) # Red -BLUE = (255, 0, 0) # Blue -WHITE = (255, 255, 255) # White - # MARKER_: Marker Data Types # MARKER_TYPE_: Marker Types # NOTE Data about each marker @@ -188,7 +181,8 @@ def __init__(self, start_res=None, focal_lengths=None): raise "Invalid resolution for camera." elif self.camera_model == 'imx219': # PI cam version 2.1 - # Warning: only full res and 1640x1232 are full image (scaled), everything else seems full-res and cropped, reducing FOV + # Warning: only full res and 1640x1232 are full image (scaled), + # everything else seems full-res and cropped, reducing FOV self.focal_lengths = (PI_2_1_CAMERA_FOCAL_LENGTHS if focal_lengths is None else focal_lengths) @@ -365,12 +359,14 @@ def _draw_bounding_box(self, frame, detections): """ polygon_is_closed = True for detection in detections: - marker_info = MARKER.by_id(detection.id, self.zone) + marker_info = MARKER(detection.id, self.zone) marker_info_colour = marker_info.bounding_box_color marker_code = detection.id - colour = (marker_info_colour - if marker_info_colour is not None - else DEFAULT_BOUNDING_BOX_COLOUR) + + # The reverse is because OpenCV expects BGR but we use RGB + colour = reversed(marker_info_colour + if marker_info_colour is not None + else DEFAULT_BOUNDING_BOX_COLOUR) # need to have this EXACT integer_corners syntax due to opencv bug # https://stackoverflow.com/questions/17241830/ @@ -471,7 +467,7 @@ def _generate_marker_properties(self, tags): detections = Detections() for tag in tags: - info = MARKER.by_id(int(tag.id), self.zone) + info = MARKER(int(tag.id), self.zone) detections.append(Marker(info, tag)) return detections @@ -484,12 +480,24 @@ def _send_to_post_process(self, capture, detections): except queue.Full: logging.warning("Skipping postprocessing as queue is full") - def detect_markers(self): + def _filter_markers(self, markers, look_for_type): + """Ducktype filtering of markers based on type or code or list of both""" + if look_for_type is not None: + if isinstance(look_for_type, Iterable): + markers = filter(lambda m: m.code in look_for_type + or m.type in look_for_type, markers) + else: + markers = filter(lambda m: m.code == look_for_type + or m.type == look_for_type, markers) + return markers + + def detect_markers(self, look_for=None): """Returns the markers the robot can see: - Gets a frame - Finds the markers - Appends RoboCon specific properties, e.g. token or arena - Sends off for post processing + - Filters and sorts the markers """ capture = self.camera.capture() @@ -500,5 +508,7 @@ def detect_markers(self): self._send_to_post_process(capture, detections) markers = self._generate_marker_properties(detections) + markers = self._filter_markers(markers, look_for) + markers = sorted(markers, key=lambda m: m.dist) return markers diff --git a/robot/wrapper.py b/robot/wrapper.py index e49b1ec..a380e34 100644 --- a/robot/wrapper.py +++ b/robot/wrapper.py @@ -20,9 +20,9 @@ class to their respecitve classes from robot import vision from robot.cytron import CytronBoard from robot.greengiant import GreenGiantInternal, GreenGiantGPIOPinList, GreenGiantMotors, _GG_SERVO_PWM_BASE, _GG_GPIO_PWM_BASE, _GG_GPIO_GPIO_BASE, _GG_SERVO_GPIO_BASE -from robot.marker_setup.teams import TEAM -from . import marker_setup -from robot.marker_setup import POEM_ON_STARTUP +from robot.game_config import TEAM +from . import game_config +from robot.game_config import POEM_ON_STARTUP _logger = logging.getLogger("robot") @@ -67,7 +67,7 @@ def __init__(self, start_enable_5v = True, ): - self.zone = marker_setup.TEAM.RUSSET + self.zone = game_config.TEAM.RUSSET self.mode = "competition" self._max_motor_voltage = max_motor_voltage @@ -169,7 +169,7 @@ def report_hardware_status(self): # print report of hardware _logger.info("------HARDWARE REPORT------") - #_logger.info("Time: %s", datetime.now().strftime('%Y-%m-%d %H:%M:%S')) + #_logger.info("Time: %s", datetime.now().strftime('%Y-%m-%d %H:%M:%S')) # no RTC on new boards, perhaps use a "run number" increment instead? _logger.info("Patch Version: ") _logger.info(battery_str) @@ -199,7 +199,7 @@ def enable_motors(self): For the PiLow series the Motors have both a power control and a enable. Generally the Power should not be switched on and off, just the enable bits. The power may - be tripped in extreame circumstances. I guess that here we want to report any + be tripped in extreame circumstances. I guess that here we want to report any reason for the motors not working, which includes power and enable """ @@ -213,7 +213,7 @@ def enable_motors(self, on): """An nice alias for set_12v""" if self._version < 10: return self._green_giant.enable_motors(on) - + @property def enable_12v(self): return self._green_giant.get_12v_acc_power() @@ -225,7 +225,7 @@ def enable_12v(self, on): @property def enable_5v(self): return self._green_giant.get_5v_acc_power() - + @enable_5v.setter def enable_5v(self, on): self._green_giant.set_5v_acc_power(on) @@ -311,10 +311,10 @@ def wait_start(self): def set_user_led(self, val=True): self._green_giant.set_user_led(val) - def see(self) -> vision.Detections: + def see(self, look_for=None) -> vision.Detections: """Take a photo, detect markers in sene, attach RoboCon specific properties""" - return self._vision.detect_markers() + return self._vision.detect_markers(look_for=look_for) def __del__(self): """Frees hardware resources held by the vision object""" diff --git a/setup.py b/setup.py index cdbcfe8..7c93982 100644 --- a/setup.py +++ b/setup.py @@ -3,7 +3,7 @@ setup( name="robot", version="2024.1", - packages=["robot", "robot.marker_setup"], + packages=["robot"], install_requires=[ ],