From b2ff29927390f047ce194ba80fcfbab009115837 Mon Sep 17 00:00:00 2001 From: Kirill Polzounov Date: Fri, 25 Feb 2022 16:54:56 -0800 Subject: [PATCH 1/3] Architecture and calibration changes --- sw/calibrate.py | 66 +++++++++----- sw/camera.py | 26 +----- sw/detector.py | 222 ++++++++++++++-------------------------------- sw/hardware.py | 13 ++- sw/info_screen.py | 7 +- sw/menu.py | 5 +- sw/procid.py | 7 +- 7 files changed, 138 insertions(+), 208 deletions(-) diff --git a/sw/calibrate.py b/sw/calibrate.py index bca253d..9839ce6 100644 --- a/sw/calibrate.py +++ b/sw/calibrate.py @@ -19,10 +19,10 @@ from env import MoabEnv from typing import Tuple from common import Vector2 -from detector import hsv_detector from controllers import pid_controller from dataclasses import dataclass, astuple from hardware import plate_angles_to_servo_positions +from detector import hsv_detector, pixels_to_meters, meters_to_pixels @dataclass @@ -106,24 +106,35 @@ def calibrate_hue(camera_fn, detector_fn, is_menu_down_fn): return CalibHue() -def calibrate_pos(camera_fn, detector_fn, hue, is_menu_down_fn): - for i in range(10): # Try and detect for 10 frames before giving up - if is_menu_down_fn(): - return CalibPos(early_quit=True) +def calibrate_pos( + camera_fn, detector_fn, get_buttons_fn, prev_plate_offsets, sleep_time=1 / 30 +): + def clip(x, low, high): + return max(0, min(high, x)) - img_frame, elapsed_time = camera_fn() - ball_detected, ((x, y), radius) = detector_fn(img_frame, hue=hue) + x_pixels, y_pixels = meters_to_pixels(prev_plate_offsets) + x_pixels, y_pixels = list(Vector2(x_pixels, y_pixels).rotate(30)) + menu_button, joy_button, joy_x, joy_y = get_buttons_fn() - # If we found a ball roughly in the center that is large enough - if ball_detected and ball_close_enough(x, y, radius): - x_offset = round(x, 3) - y_offset = round(y, 3) + while True: + if menu_button: + return CalibPos(early_quit=True) + if joy_button: + break + + img_frame, _ = camera_fn() + lx, ly = img_frame.shape[:2] + x_pixels = int(x_pixels + round(joy_x * 2)) + y_pixels = int(y_pixels + -round(joy_y * 2)) + _ = detector_fn(img_frame, hue=0, debug=True, crosshairs=(x_pixels, y_pixels)) - log.info(f"Offset calibrated: [{x_offset:.3f}, {y_offset:.3f}]") - return CalibPos(position=(x_offset, y_offset), success=True) + time.sleep(sleep_time) + menu_button, joy_button, joy_x, joy_y = get_buttons_fn() - log.warning(f"Offset calibration failed.") - return CalibPos() + # Rotate by -30 degrees then convert to meters + x_offset, y_offset = pixels_to_meters(Vector2(x_pixels, y_pixels).rotate(-30)) + log.info(f"Offset calibrated: [{x_offset:.3f}, {y_offset:.3f}]") + return CalibPos(position=(x_offset, y_offset), success=True) def calibrate_servo_offsets(pid_fn, env, stationary_vel=0.005, time_limit=20): @@ -216,6 +227,9 @@ def run_calibration(env, pid_fn, calibration_file): camera_fn = hardware.camera detector_fn = hardware.detector + # Get old calibration (occasionally useful as a starting point) + calibration_dict = read_calibration(calibration_file) + def is_menu_down(hardware=hardware) -> bool: return hardware.get_buttons().menu_button @@ -224,15 +238,28 @@ def is_menu_down(hardware=hardware) -> bool: # Display message and wait for joystick hardware.display( - "put ball on stand\nclick joystick", - # "Place ball in\ncenter using\nclear stand.\n\n" "Click joystick\nwhen ready." + "move crosshairs\nclick joystick", scrolling=True, ) - buttons = wait_for_joystick_or_menu(hardware) - if buttons.menu_button: # Early quit + + # Calibrate position + pos_calib = calibrate_pos( + camera_fn, detector_fn, hardware.get_buttons, calibration_dict["plate_offsets"] + ) + if pos_calib.early_quit: hardware.go_up() return + hardware.display( + "put ball on stand\nclick joystick", + # "Place ball in\ncenter using\nclear stand.\n\n" "Click joystick\nwhen ready." + scrolling=True, + ) + # buttons = wait_for_joystick_or_menu(hardware) + # if buttons.menu_button: # Early quit + # hardware.go_up() + # return + hardware.display("Calibrating...") hue_calib = calibrate_hue(camera_fn, detector_fn, is_menu_down) if hue_calib.early_quit: @@ -246,7 +273,6 @@ def is_menu_down(hardware=hardware) -> bool: return # Save calibration - calibration_dict = read_calibration(calibration_file) calibration_dict["ball_hue"] = hue_calib.hue calibration_dict["plate_offsets"] = pos_calib.position x_offset, y_offset = pos_calib.position diff --git a/sw/camera.py b/sw/camera.py index 043fc17..f61a63b 100644 --- a/sw/camera.py +++ b/sw/camera.py @@ -6,10 +6,11 @@ """ import cv2 +import time import threading import numpy as np -from time import time +from typing import Union, Tuple # Link to raspicam_cv implementation for mapping values @@ -22,8 +23,6 @@ def __init__( brightness=60, contrast=100, frequency=30, - x_offset_pixels=0.0, - y_offset_pixels=0.0, auto_exposure=True, exposure=50, # int for manual (each 1 is 100µs of exposure) ): @@ -38,9 +37,6 @@ def __init__( self.exposure = exposure self.prev_time = 0.0 self.source = None - self.last_frame = None - self.x_offset_pixels = x_offset_pixels - self.y_offset_pixels = y_offset_pixels def start(self): self.source = cv2.VideoCapture(self.device_id) @@ -69,28 +65,12 @@ def __call__(self): raise Exception("Using camera before it has been started.") # Calculate the time elapsed since the last sensor snapshot - curr_time = time() + curr_time = time.time() elapsed_time = curr_time - self.prev_time self.prev_time = curr_time ret, frame = self.source.read() if ret: - w, h = 384, 288 - d = 256 # Our final "destination" rectangle is 256x256 - - # Ensure the offset crops are possible - x_offset_pixels = min(self.x_offset_pixels, (w / 2 - d / 2)) - x_offset_pixels = max(self.x_offset_pixels, -(w / 2 - d / 2)) - y_offset_pixels = min(self.y_offset_pixels, (h / 2 - d / 2)) - y_offset_pixels = max(self.y_offset_pixels, -(h / 2 - d / 2)) - - # Calculate the starting point in x & y - x = int((w / 2 - d / 2) + x_offset_pixels) - y = int((h / 2 - d / 2) + y_offset_pixels) - - frame = frame[y : y + d, x : x + d] - # frame = frame[:-24, 40:-80] # Crop so middle of plate is middle of image - # cv2.resize(frame, (256, 256)) # Crop off edges to make image (256, 256) return frame, elapsed_time else: raise ValueError("Could not get the next frame") diff --git a/sw/detector.py b/sw/detector.py index df4dd00..f61a63b 100644 --- a/sw/detector.py +++ b/sw/detector.py @@ -2,163 +2,75 @@ # Licensed under the MIT License. """ -HSV filtering ball detector +A sensor that uses OpenCV for capture """ import cv2 -import math +import time +import threading import numpy as np -from hsv import hue_to_bgr -from huemask import hue_mask -from common import Vector2, CircleFeature, Calibration -from typing import List, Optional - - -def pixels_to_meters(vec, frame_size=256, field_of_view=1.05): - # The plate is default roughly 105% of the field of view - plate_diameter_meters = 0.225 - plate_diameter_pixels = frame_size * field_of_view - conversion = plate_diameter_meters / plate_diameter_pixels - return np.asarray(vec) * conversion - - -def meters_to_pixels(vec, frame_size=256, field_of_view=1.05): - # The plate is default roughly 105% of the field of view - plate_diameter_meters = 0.225 - plate_diameter_pixels = frame_size * field_of_view - conversion = plate_diameter_meters / plate_diameter_pixels - return np.int_(np.asarray(vec) / conversion) # Note: pixels are only ever ints - - -def pixel_to_meter_ratio(frame_size=256, field_of_view=1.05): - # The plate is default roughly 105% of the field of view - plate_diameter_meters = 0.225 - plate_diameter_pixels = frame_size * field_of_view - conversion = plate_diameter_meters / plate_diameter_pixels - return conversion - - -def draw_ball(img, center, radius, hue): - bgr = hue_to_bgr(hue) - # 45 -> hsl(45°, 75%, 50%) - cv2.circle(img, center, 2, bgr, 2) - cv2.circle(img, center, int(radius), bgr, 2) - return img - - -def save_img(filepath, img, rotated=False, quality=80): - if rotated: - # Rotate the image -30 degrees so it looks normal - w, h = img.shape[:2] - center = (w / 2, h / 2) - M = cv2.getRotationMatrix2D(center, 30, 1.0) - img = cv2.warpAffine(img, M, (w, h)) - img = img[::-1, :, :] # Mirror along x axis - - cv2.imwrite( - filepath, - img, - [cv2.IMWRITE_JPEG_QUALITY, quality], - ) - - -def hsv_detector( - calibration=None, - frame_size=256, - kernel_size=[5, 5], - ball_min=0.06, - ball_max=0.22, - hue=None, # hue [0..255] - debug=False, -): - if calibration is None: - calibration = Calibration() - # if we haven't been overridden, use ballHue from calibration - if hue is None: - hue = calibration.ball_hue - kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, tuple(kernel_size)) - - def detect_features(img, hue=hue, debug=debug, filename="/tmp/camera/frame.jpg"): - # covert to HSV space - img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - - # The hue_mask function follows CV2 convention and hue is in the range - # [0, 180] instead of [0, 360] - # run through each triplet and perform our masking filter on it. - # hue_mask coverts the hsv image into a grayscale image with a - # bandpass applied centered around hue, with width sigma - hue_mask(img_hsv, hue / 2, 0.03, 60.0, 1.5) - - # convert to b&w mask from grayscale image - mask = cv2.inRange( - img_hsv, np.array((200, 200, 200)), np.array((255, 255, 255)) - ) - - # expand b&w image with a dialation filter - mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) - - contours = cv2.findContours( - mask, - cv2.RETR_EXTERNAL, - cv2.CHAIN_APPROX_SIMPLE, - )[-2] - - if len(contours) > 0: - contour_peak = max(contours, key=cv2.contourArea) - ((x_obs, y_obs), radius) = cv2.minEnclosingCircle(contour_peak) - - # Determine if ball size is the appropriate size - norm_radius = radius / frame_size - if ball_min < norm_radius < ball_max: - ball_detected = True - - # Convert from pixels to absolute with 0,0 as center of detected plate - x = x_obs - frame_size // 2 - y = y_obs - frame_size // 2 - - if debug: - ball_center_pixels = (int(x_obs), int(y_obs)) - draw_ball(img, ball_center_pixels, radius, hue) - save_img(filename, img, rotated=False, quality=80) - - # Rotate the x, y coordinates by -30 degrees - center = Vector2(x, y).rotate(np.radians(-30)) - center = pixels_to_meters(center, frame_size) - return ball_detected, (center, radius) - - # If there were no contours or no contours the size of the ball - ball_detected = False - if debug: - save_img(filename, img, rotated=False, quality=80) - return ball_detected, (Vector2(0, 0), 0.0) - - return detect_features - - -def circle_test_detector(hue=44, debug=False, *args, **kwargs): - angle = 0 - time = 1 - frequency = 30 # in Hz - scale = pixel_to_meter_ratio() - radius = 256 * 0.4 - ball_radius_pixels = 256 * 0.1 - - def detect_features(img, hue=hue, debug=debug, filename="/tmp/camera/frame.jpg"): - nonlocal angle - angle += (1 / (time * frequency)) * (2 * np.pi) - x_pixels, y_pixels = (radius * np.sin(angle), radius * np.cos(angle)) - x_pixels += 256 / 2 - y_pixels += 256 / 2 - - if debug: - ball_center_pixels = (int(x_pixels), int(y_pixels)) - print(ball_center_pixels) - draw_ball(img, ball_center_pixels, ball_radius_pixels, hue) - save_img(filename, img, rotated=False, quality=80) - - x, y = x_pixels * scale, y_pixels * scale - ball_detected = True - return ball_detected, (Vector2(x, y), ball_radius_pixels * scale) - - return detect_features +from typing import Union, Tuple + + +# Link to raspicam_cv implementation for mapping values +# https://github.com/cedricve/raspicam/blob/651c56418a5a594fc12f1414eb14f2b899729cb1/src/raspicam_cv.h#L108 +class OpenCVCameraSensor: + def __init__( + self, + device_id=0, + rotation=0, + brightness=60, + contrast=100, + frequency=30, + auto_exposure=True, + exposure=50, # int for manual (each 1 is 100µs of exposure) + ): + self.device_id = device_id + # self.width = width + # self.height = height + self.rotation = rotation + self.brightness = brightness + self.contrast = contrast + self.frequency = frequency + self.auto_exposure = auto_exposure + self.exposure = exposure + self.prev_time = 0.0 + self.source = None + + def start(self): + self.source = cv2.VideoCapture(self.device_id) + if self.source: + self.source.set(cv2.CAP_PROP_FRAME_WIDTH, 384) + self.source.set(cv2.CAP_PROP_FRAME_HEIGHT, 288) + self.source.set(cv2.CAP_PROP_FPS, self.frequency) + self.source.set(cv2.CAP_PROP_MODE, 0) # Not meant to be configurable + self.source.set(cv2.CAP_PROP_BRIGHTNESS, self.brightness) + self.source.set(cv2.CAP_PROP_CONTRAST, self.contrast) + self.source.set( + cv2.CAP_PROP_AUTO_EXPOSURE, 0.25 if self.auto_exposure else 0.75 + ) + self.source.set(cv2.CAP_PROP_EXPOSURE, self.exposure) + else: + raise Exception("Couldn't create camera.") + + def stop(self): + if self.source: + self.source.release() + self.source = None + + def __call__(self): + # safety check + if self.source is None: + raise Exception("Using camera before it has been started.") + + # Calculate the time elapsed since the last sensor snapshot + curr_time = time.time() + elapsed_time = curr_time - self.prev_time + self.prev_time = curr_time + + ret, frame = self.source.read() + if ret: + return frame, elapsed_time + else: + raise ValueError("Could not get the next frame") diff --git a/sw/hardware.py b/sw/hardware.py index ed742d6..b1ffbf0 100644 --- a/sw/hardware.py +++ b/sw/hardware.py @@ -6,6 +6,7 @@ import json import numpy as np +from common import Vector2 from detector import hsv_detector from typing import Tuple, Optional from camera import OpenCVCameraSensor @@ -91,9 +92,9 @@ def reset_calibration(self, calibration_file=None): calib = json.load(f) else: # Use defaults calib = { - "ball_hue": 44, - "plate_offsets": (0.0, 0.0), - "servo_offsets": (0.0, 0.0, 0.0), + "ball_hue": 44, # Hue + "plate_offsets": (0.0, 0.0), # X,Y offset in meters + "servo_offsets": (0.0, 0.0, 0.0), # Servo angle offset in degrees } self.plate_offsets = calib["plate_offsets"] @@ -154,4 +155,8 @@ def step(self, pitch, roll) -> Buttons: frame, elapsed_time = self.camera() buttons = self.hat.get_buttons() ball_detected, (ball_center, ball_radius) = self.detector(frame, hue=self.hue) - return ball_center, ball_detected, buttons + + ball_center.x -= self.plate_offsets[0] + ball_center.y -= self.plate_offsets[1] + + return ball_center, ball_detected, buttons \ No newline at end of file diff --git a/sw/info_screen.py b/sw/info_screen.py index 05aa6dc..0f9ab6a 100644 --- a/sw/info_screen.py +++ b/sw/info_screen.py @@ -4,12 +4,13 @@ # Licensed under the MIT License. import os -from random import * -from time import * import socket import logging as log -from hat import Hat, Icon + +from time import sleep from env import MoabEnv +from hat import Hat, Icon +from random import randint def _get_host_ip(): diff --git a/sw/menu.py b/sw/menu.py index b230f34..4ef58e2 100755 --- a/sw/menu.py +++ b/sw/menu.py @@ -6,13 +6,14 @@ import os import sys import time +import json import yaml +import json import click +import docker import procid import logging import subprocess -import json -import docker from hat import Icon from enum import Enum diff --git a/sw/procid.py b/sw/procid.py index 4b8ae88..4bd8ec4 100644 --- a/sw/procid.py +++ b/sw/procid.py @@ -1,7 +1,12 @@ #!/usr/bin/env python3 -import sys, os, time, errno + +# Copyright (c) Microsoft Corporation. +# Licensed under the MIT License. + import psutil import logging as log +import sys, os, time, errno + from psutil import Process, signal From df2c48a86226d93ad98f80992d1b66cafe757d79 Mon Sep 17 00:00:00 2001 From: Kirill Polzounov Date: Mon, 28 Feb 2022 13:30:46 -0800 Subject: [PATCH 2/3] Fixed problem with crosshairs --- sw/calibrate.py | 108 ++++----------------- sw/detector.py | 249 +++++++++++++++++++++++++++++++++++------------- sw/hardware.py | 12 ++- 3 files changed, 210 insertions(+), 159 deletions(-) diff --git a/sw/calibrate.py b/sw/calibrate.py index 9839ce6..0c2ff00 100644 --- a/sw/calibrate.py +++ b/sw/calibrate.py @@ -8,7 +8,6 @@ """ import os -import cv2 import time import json import argparse @@ -22,7 +21,7 @@ from controllers import pid_controller from dataclasses import dataclass, astuple from hardware import plate_angles_to_servo_positions -from detector import hsv_detector, pixels_to_meters, meters_to_pixels +from detector import pixels_to_meters, meters_to_pixels @dataclass @@ -237,41 +236,24 @@ def is_menu_down(hardware=hardware) -> bool: hardware.set_angles(0, 0) # Display message and wait for joystick - hardware.display( - "move crosshairs\nclick joystick", - scrolling=True, - ) + hardware.display("move crosshairs\nclick joystick", scrolling=True) # Calibrate position - pos_calib = calibrate_pos( - camera_fn, detector_fn, hardware.get_buttons, calibration_dict["plate_offsets"] - ) + prev_plate_offsets = calibration_dict["plate_offsets"] + buttons = hardware.get_buttons + pos_calib = calibrate_pos(camera_fn, detector_fn, buttons, prev_plate_offsets) if pos_calib.early_quit: hardware.go_up() return - hardware.display( - "put ball on stand\nclick joystick", - # "Place ball in\ncenter using\nclear stand.\n\n" "Click joystick\nwhen ready." - scrolling=True, - ) - # buttons = wait_for_joystick_or_menu(hardware) - # if buttons.menu_button: # Early quit - # hardware.go_up() - # return - + # Calibrate hue + hardware.display("put ball on stand\nclick joystick", scrolling=True) hardware.display("Calibrating...") hue_calib = calibrate_hue(camera_fn, detector_fn, is_menu_down) if hue_calib.early_quit: hardware.go_up() return - # Calibrate position - pos_calib = calibrate_pos(camera_fn, detector_fn, hue_calib.hue, is_menu_down) - if pos_calib.early_quit: - hardware.go_up() - return - # Save calibration calibration_dict["ball_hue"] = hue_calib.hue calibration_dict["plate_offsets"] = pos_calib.position @@ -287,16 +269,18 @@ def is_menu_down(hardware=hardware) -> bool: elif not (pos_calib.success or hue_calib.success): # or servo_calib.success): hardware.display("Calibration failed\nClick menu...", scrolling=True) else: - hue_str = ( - f"Hue calib:\nsuccessful\nBall hue = {hue_calib.hue}\n\n" - if hue_calib.success - else "Hue calib:\nfailed\n\n" - ) - pos_str = ( - f"Position \ncalib:\nsuccessful\nPosition = \n({100*x_offset:.1f}, {100*y_offset:.1f})cm\n\n" - if hue_calib.success - else "(X, Y) calib:\nfailed\n\n" - ) + # Calibration partially succeeded + if hue_calib.success: + hue_str = f"Ball hue = {hue_calib.hue}\n\n" + else: + hue_str = "Hue calib:\nfailed\n\n" + + if pos_calib.success: + pos_str = f"Position = \n" + pos_str += f"({100*x_offset:.1f}, {100*y_offset:.1f})cm\n\n" + else: + pos_str = "(X, Y) calib:\nfailed\n\n" + hardware.display( "Calibration\npartially succeeded\n\n" + hue_str @@ -325,60 +309,6 @@ def is_menu_down(hardware=hardware) -> bool: hardware.go_up() -def run_servo_calibration(env, pid_fn, calibration_file): - # Warning: servo calib works but doesn't currently give a good calibration - raise NotImplementedError - - # Get some hidden things from env - hardware = env.hardware - camera_fn = hardware.camera - detector_fn = hardware.detector - - # Start the calibration with uncalibrated servos - hardware.servo_offsets = (0, 0, 0) - # lift plate up fist - hardware.set_angles(0, 0) - - # Calibrate servo offsets - hardware.display( - "Calibarating\nservos\n\n" - "Place ball in\ncenter without\n stand.\n\n" - "Click joystick\nto continue.", - scrolling=True, - ) - buttons = wait_for_joystick_or_menu(hardware) - if buttons.menu_button: # Early quit - hardware.go_up() - return - - hardware.display("Calibrating\nservos...", scrolling=True) - servo_calib = calibrate_servo_offsets(pid_fn, env) - - # Save calibration - calibration_dict = read_calibration(calibration_file) - calibration_dict["servo_offsets"] = servo_calib.servos - s1, s2, s3 = servo_calib.servos - write_calibration(calibration_dict) - - # Update the environment to use the new calibration - # Warning! This mutates the state! - env.reset_calibration(calibration_file=calibration_file) - - if servo_calib.success: - hardware.display( - f"servo offsets =\n({s1:.2f}, {s2:.2f}, {s3:.2f})\n\n" - "Click menu\nto return...\n", - scrolling=True, - ) - print(f"servo offsets =\n({s1:.2f}, {s2:.2f}, {s3:.2f})") - else: - hardware.display( - "Calibration\nfailed\n\nClick menu\nto return...", scrolling=True - ) - - hardware.go_up() - - def calibrate_controller(**kwargs): run_calibration( kwargs["env"], diff --git a/sw/detector.py b/sw/detector.py index f61a63b..f03ed20 100644 --- a/sw/detector.py +++ b/sw/detector.py @@ -2,75 +2,192 @@ # Licensed under the MIT License. """ -A sensor that uses OpenCV for capture +HSV filtering ball detector """ import cv2 -import time -import threading +import math import numpy as np -from typing import Union, Tuple - - -# Link to raspicam_cv implementation for mapping values -# https://github.com/cedricve/raspicam/blob/651c56418a5a594fc12f1414eb14f2b899729cb1/src/raspicam_cv.h#L108 -class OpenCVCameraSensor: - def __init__( - self, - device_id=0, - rotation=0, - brightness=60, - contrast=100, - frequency=30, - auto_exposure=True, - exposure=50, # int for manual (each 1 is 100µs of exposure) +from hsv import hue_to_bgr +from huemask import hue_mask +from typing import List, Optional +from common import Vector2, CircleFeature, Calibration + + +def pixels_to_meters(vec, frame_size=256, field_of_view=1.05): + # The plate is default roughly 105% of the field of view + plate_diameter_meters = 0.225 + plate_diameter_pixels = frame_size * field_of_view + conversion = plate_diameter_meters / plate_diameter_pixels + return np.asarray(vec) * conversion + + +def meters_to_pixels(vec, frame_size=256, field_of_view=1.0): + # The plate is default roughly 105% of the field of view + plate_diameter_meters = 0.225 + plate_diameter_pixels = frame_size * field_of_view + conversion = plate_diameter_meters / plate_diameter_pixels + return np.int_(np.asarray(vec) / conversion) # Note: pixels are only ever ints + + +def pixel_to_meter_ratio(frame_size=256, field_of_view=1.05): + # The plate is default roughly 105% of the field of view + plate_diameter_meters = 0.225 + plate_diameter_pixels = frame_size * field_of_view + conversion = plate_diameter_meters / plate_diameter_pixels + return conversion + + +def draw_ball(img, center, radius, hue): + bgr = hue_to_bgr(hue) + # 45 -> hsl(45°, 75%, 50%) + cv2.circle(img, center, 2, bgr, 2) + cv2.circle(img, center, int(radius), bgr, 2) + return img + + +def draw_crosshairs(img, crosshairs): + x, y = crosshairs + lx, ly = img.shape[:2] + cv2.line(img, (0, ly // 2 + y), (ly, ly // 2 + y), (0, 0, 255), 2) # X axis + cv2.line(img, (lx // 2 + x, 0), (lx // 2 + x, lx), (0, 0, 255), 2) # Y axis + return img + + +def save_img(filepath, img, rotated=False, quality=80): + if rotated: + # Rotate the image -30 degrees so it looks normal + w, h = img.shape[:2] + center = (w / 2, h / 2) + M = cv2.getRotationMatrix2D(center, 30, 1.0) + img = cv2.warpAffine(img, M, (w, h)) + img = img[::-1, :, :] # Mirror along x axis + + cv2.imwrite( + filepath, + img, + [cv2.IMWRITE_JPEG_QUALITY, quality], + ) + + +def hsv_detector( + calibration=None, + frame_size=256, + kernel_size=[5, 5], + ball_min=0.06, + ball_max=0.22, + hue=None, # hue [0..255] + debug=False, +): + if calibration is None: + calibration = Calibration() + # if we haven't been overridden, use ballHue from calibration + if hue is None: + hue = calibration.ball_hue + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, tuple(kernel_size)) + + def detect_features( + img, + hue=hue, + debug=debug, + filename="/tmp/camera/frame.jpg", + crosshairs=None, + axis=None, ): - self.device_id = device_id - # self.width = width - # self.height = height - self.rotation = rotation - self.brightness = brightness - self.contrast = contrast - self.frequency = frequency - self.auto_exposure = auto_exposure - self.exposure = exposure - self.prev_time = 0.0 - self.source = None - - def start(self): - self.source = cv2.VideoCapture(self.device_id) - if self.source: - self.source.set(cv2.CAP_PROP_FRAME_WIDTH, 384) - self.source.set(cv2.CAP_PROP_FRAME_HEIGHT, 288) - self.source.set(cv2.CAP_PROP_FPS, self.frequency) - self.source.set(cv2.CAP_PROP_MODE, 0) # Not meant to be configurable - self.source.set(cv2.CAP_PROP_BRIGHTNESS, self.brightness) - self.source.set(cv2.CAP_PROP_CONTRAST, self.contrast) - self.source.set( - cv2.CAP_PROP_AUTO_EXPOSURE, 0.25 if self.auto_exposure else 0.75 - ) - self.source.set(cv2.CAP_PROP_EXPOSURE, self.exposure) - else: - raise Exception("Couldn't create camera.") - - def stop(self): - if self.source: - self.source.release() - self.source = None - - def __call__(self): - # safety check - if self.source is None: - raise Exception("Using camera before it has been started.") - - # Calculate the time elapsed since the last sensor snapshot - curr_time = time.time() - elapsed_time = curr_time - self.prev_time - self.prev_time = curr_time - - ret, frame = self.source.read() - if ret: - return frame, elapsed_time - else: - raise ValueError("Could not get the next frame") + # covert to HSV space + img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + # The hue_mask function follows CV2 convention and hue is in the range + # [0, 180] instead of [0, 360] + # run through each triplet and perform our masking filter on it. + # hue_mask coverts the hsv image into a grayscale image with a + # bandpass applied centered around hue, with width sigma + hue_mask(img_hsv, hue / 2, 0.03, 60.0, 1.5) + + # convert to b&w mask from grayscale image + mask = cv2.inRange( + img_hsv, np.array((200, 200, 200)), np.array((255, 255, 255)) + ) + + # expand b&w image with a dialation filter + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) + + contours = cv2.findContours( + mask, + cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_SIMPLE, + )[-2] + + if len(contours) > 0: + contour_peak = max(contours, key=cv2.contourArea) + ((x_obs, y_obs), radius) = cv2.minEnclosingCircle(contour_peak) + + # Determine if ball size is the appropriate size + norm_radius = radius / frame_size + if ball_min < norm_radius < ball_max: + ball_detected = True + + # Convert from pixels to absolute with 0,0 as center of detected plate + x = int(x_obs - frame_size // 2) + y = int(y_obs - frame_size // 2) + + if debug: + ball_center_pixels = (int(x_obs), int(y_obs)) + draw_ball(img, ball_center_pixels, radius, hue) + if crosshairs is not None: + draw_crosshairs(img, crosshairs) + if axis is not None: + # Draw the axis (xy plate offsets) + x, y = meters_to_pixels(axis) + x, y = list(Vector2(x, y).rotate(30)) + draw_crosshairs(img, (int(x), int(y))) + save_img(filename, img, rotated=False, quality=80) + + # Rotate the x, y coordinates by -30 degrees + center = Vector2(x, y).rotate(np.radians(-30)) + center = pixels_to_meters(center, frame_size) + return ball_detected, (center, radius) + + # If there were no contours or no contours the size of the ball + ball_detected = False + if debug: + if crosshairs is not None: + draw_crosshairs(img, crosshairs) + if axis is not None: + # Draw the axis (xy plate offsets) + x, y = meters_to_pixels(axis) + x, y = list(Vector2(x, y).rotate(30)) + draw_crosshairs(img, (int(x), int(y))) + save_img(filename, img, rotated=False, quality=80) + return ball_detected, (Vector2(0, 0), 0.0) + + return detect_features + + +def circle_test_detector(hue=44, debug=False, *args, **kwargs): + angle = 0 + time = 1 + frequency = 30 # in Hz + scale = pixel_to_meter_ratio() + radius = 256 * 0.4 + ball_radius_pixels = 256 * 0.1 + + def detect_features(img, hue=hue, debug=debug, filename="/tmp/camera/frame.jpg"): + nonlocal angle + angle += (1 / (time * frequency)) * (2 * np.pi) + x_pixels, y_pixels = (radius * np.sin(angle), radius * np.cos(angle)) + x_pixels += 256 / 2 + y_pixels += 256 / 2 + + if debug: + ball_center_pixels = (int(x_pixels), int(y_pixels)) + print(ball_center_pixels) + draw_ball(img, ball_center_pixels, ball_radius_pixels, hue) + save_img(filename, img, rotated=False, quality=80) + + x, y = x_pixels * scale, y_pixels * scale + ball_detected = True + return ball_detected, (Vector2(x, y), ball_radius_pixels * scale) + + return detect_features \ No newline at end of file diff --git a/sw/hardware.py b/sw/hardware.py index b1ffbf0..e1aec61 100644 --- a/sw/hardware.py +++ b/sw/hardware.py @@ -154,9 +154,13 @@ def step(self, pitch, roll) -> Buttons: self.set_angles(pitch, roll) frame, elapsed_time = self.camera() buttons = self.hat.get_buttons() - ball_detected, (ball_center, ball_radius) = self.detector(frame, hue=self.hue) - ball_center.x -= self.plate_offsets[0] - ball_center.y -= self.plate_offsets[1] + detected, (center, radius) = self.detector( + frame, hue=self.hue, axis=self.plate_offsets + ) - return ball_center, ball_detected, buttons \ No newline at end of file + center.x -= self.plate_offsets[0] + center.y -= self.plate_offsets[1] + + # Return the ball center, ball betected, and buttons + return center, detected, buttons \ No newline at end of file From 10bef6343bb816a650ab92fbac23e42880fecccf Mon Sep 17 00:00:00 2001 From: Kirill Polzounov Date: Sun, 6 Mar 2022 06:53:09 -0800 Subject: [PATCH 3/3] Calibrate, pixel offsets all working --- sw/calibrate.py | 57 +++++++++---------------------- sw/common.py | 91 +++++++++++++++++++++++++++++++++++++++++++++---- sw/detector.py | 58 +++++++++++++++---------------- sw/hardware.py | 31 +++++------------ 4 files changed, 138 insertions(+), 99 deletions(-) diff --git a/sw/calibrate.py b/sw/calibrate.py index 0c2ff00..45352f4 100644 --- a/sw/calibrate.py +++ b/sw/calibrate.py @@ -17,11 +17,11 @@ from env import MoabEnv from typing import Tuple -from common import Vector2 from controllers import pid_controller from dataclasses import dataclass, astuple from hardware import plate_angles_to_servo_positions from detector import pixels_to_meters, meters_to_pixels +from common import Vector2, write_calibration, read_calibration @dataclass @@ -35,8 +35,8 @@ def __iter__(self): @dataclass -class CalibPos: - position: Tuple[float, float] = (0.0, 0.0) +class CalibPixels: + offsets: Tuple[int, int] = (0, 0) success: bool = False early_quit: bool = False # If menu is pressed before the calibration is complete @@ -106,18 +106,19 @@ def calibrate_hue(camera_fn, detector_fn, is_menu_down_fn): def calibrate_pos( - camera_fn, detector_fn, get_buttons_fn, prev_plate_offsets, sleep_time=1 / 30 + camera_fn, detector_fn, get_buttons_fn, prev_pixel_offsets, sleep_time=1 / 30 ): def clip(x, low, high): return max(0, min(high, x)) - x_pixels, y_pixels = meters_to_pixels(prev_plate_offsets) - x_pixels, y_pixels = list(Vector2(x_pixels, y_pixels).rotate(30)) + img_frame, _ = camera_fn() + # Starting point for x and y pixels + x_pixels, y_pixels = prev_pixel_offsets menu_button, joy_button, joy_x, joy_y = get_buttons_fn() while True: if menu_button: - return CalibPos(early_quit=True) + return CalibPixels(early_quit=True) if joy_button: break @@ -130,10 +131,8 @@ def clip(x, low, high): time.sleep(sleep_time) menu_button, joy_button, joy_x, joy_y = get_buttons_fn() - # Rotate by -30 degrees then convert to meters - x_offset, y_offset = pixels_to_meters(Vector2(x_pixels, y_pixels).rotate(-30)) - log.info(f"Offset calibrated: [{x_offset:.3f}, {y_offset:.3f}]") - return CalibPos(position=(x_offset, y_offset), success=True) + log.info(f"Offset calibrated: ({x_pixels}, {y_pixels})") + return CalibPixels(offsets=(x_pixels, y_pixels), success=True) def calibrate_servo_offsets(pid_fn, env, stationary_vel=0.005, time_limit=20): @@ -160,7 +159,7 @@ def calibrate_servo_offsets(pid_fn, env, stationary_vel=0.005, time_limit=20): vel_y_hist.append(vel_y) prev_100_x = np.mean(np.abs(vel_x_hist[-100:])) prev_100_y = np.mean(np.abs(vel_y_hist[-100:])) - print("Prev 100: ", (prev_100_x, prev_100_y)) + # print("Prev 100: ", (prev_100_x, prev_100_y)) # If the average velocity for the last 100 timesteps is under the limit if (prev_100_x < stationary_vel) and (prev_100_y < stationary_vel): @@ -179,30 +178,6 @@ def calibrate_servo_offsets(pid_fn, env, stationary_vel=0.005, time_limit=20): return CalibServos() -def write_calibration(calibration_dict, calibration_file="bot.json"): - log.info("Writing calibration.") - - # write out stuff - with open(calibration_file, "w+") as outfile: - log.info(f"Creating calibration file {calibration_file}") - json.dump(calibration_dict, outfile, indent=4, sort_keys=True) - - -def read_calibration(calibration_file="bot.json"): - log.info("Reading previous calibration.") - - if os.path.isfile(calibration_file): - with open(calibration_file, "r") as f: - calibration_dict = json.load(f) - else: # Use defaults - calibration_dict = { - "ball_hue": 44, - "plate_offsets": (0.0, 0.0), - "servo_offsets": (0.0, 0.0, 0.0), - } - return calibration_dict - - def wait_for_joystick_or_menu(hardware, sleep_time=1 / 30): """Waits for either the joystick or the menu. Returns the buttons""" while True: @@ -239,9 +214,9 @@ def is_menu_down(hardware=hardware) -> bool: hardware.display("move crosshairs\nclick joystick", scrolling=True) # Calibrate position - prev_plate_offsets = calibration_dict["plate_offsets"] + prev_pixel_offsets = calibration_dict["pixel_offsets"] buttons = hardware.get_buttons - pos_calib = calibrate_pos(camera_fn, detector_fn, buttons, prev_plate_offsets) + pos_calib = calibrate_pos(camera_fn, detector_fn, buttons, prev_pixel_offsets) if pos_calib.early_quit: hardware.go_up() return @@ -256,8 +231,8 @@ def is_menu_down(hardware=hardware) -> bool: # Save calibration calibration_dict["ball_hue"] = hue_calib.hue - calibration_dict["plate_offsets"] = pos_calib.position - x_offset, y_offset = pos_calib.position + calibration_dict["pixel_offsets"] = pos_calib.offsets + x_offset, y_offset = pos_calib.offsets write_calibration(calibration_dict) # Update the environment to use the new calibration @@ -277,7 +252,7 @@ def is_menu_down(hardware=hardware) -> bool: if pos_calib.success: pos_str = f"Position = \n" - pos_str += f"({100*x_offset:.1f}, {100*y_offset:.1f})cm\n\n" + pos_str += f"({x_offset}, {y_offset})pix\n\n" else: pos_str = "(X, Y) calib:\nfailed\n\n" diff --git a/sw/common.py b/sw/common.py index 804e30e..ca57a8c 100644 --- a/sw/common.py +++ b/sw/common.py @@ -1,7 +1,11 @@ # Copyright (c) Microsoft Corporation. # Licensed under the MIT License. +import os import math +import json +import logging as log + from dataclasses import dataclass @@ -91,10 +95,19 @@ def rotate(self, theta, o=None): y = o.y + math.sin(theta) * (self.x - o.x) + math.cos(theta) * (self.y - o.y) return Vector2(x, y) + def rotate_deg(self, theta, o=None): + return self.rotate(math.radians(theta), o=o) + def __iter__(self): yield self.x yield self.y + def __list__(self): + return [self.x, self.y] + + def __tuple__(self): + return (self.x, self.y) + def to_int_tuple(self): return (int(self.x), int(self.y)) @@ -113,9 +126,75 @@ class CircleFeature: radius = 0.0 -@dataclass -class Calibration: - ball_hue = 22 - plate_y_offset = 0.0 # -0.016 - plate_x_offset = 0.0 # -0.092 - rotation = -30.0 +def write_calibration(calibration_dict, calibration_file="bot.json"): + print("Writing calibration to file. File is:", calibration_file) + log.info("Writing calibration.") + + # write out stuff + with open(calibration_file, "w+") as outfile: + log.info(f"Creating calibration file {calibration_file}") + json.dump(calibration_dict, outfile, indent=4, sort_keys=True) + + print("Calibration written.") + + +def validate_calibration(calibration_dict, calibration_file): + """Ensure that the calibration is valid. If not fix it.""" + bh = calibration_dict.get("ball_hue") + po = calibration_dict.get("plate_offsets") + pix = calibration_dict.get("pixel_offsets") + so = calibration_dict.get("servo_offsets") + + val_bh = bh is not None and type(bh) == int and 0 <= bh < 360 + # Old calibration files have meter offsets not pixel offsets, ie should not exist + val_po = po is None + # New calibration files have pixel offsets + val_pix = ( + pix is not None + and (type(pix) == tuple or type(pix) == list) + and len(pix) == 2 + and type(pix[0]) == int + and type(pix[1]) == int + ) + val_so = ( + so is not None + and (type(so) == tuple or type(so) == list) + and len(so) == 3 + and (type(so[0]) == int or type(so[0]) == float) + and (type(so[1]) == int or type(so[1]) == float) + and (type(so[2]) == int or type(so[2]) == float) + ) + if not (val_bh and val_po and val_pix and val_so): + log.info("Calibration file is invalid. Fixing.") + log.info("Previous calibration:", calibration_dict) + + # Set defaults if something is wrong + if not val_bh: + calibration_dict["ball_hue"] = 44 + if not val_po: + del calibration_dict["plate_offsets"] + if not val_pix: + calibration_dict["pixel_offsets"] = (0, 0) + if not val_so: + calibration_dict["servo_offsets"] = (0.0, 0.0, 0.0) + write_calibration(calibration_dict, calibration_file) + + log.info("Fixed calibration:", calibration_dict) + + return calibration_dict + + +def read_calibration(calibration_file="bot.json"): + log.info("Reading previous calibration.") + + if os.path.isfile(calibration_file): + with open(calibration_file, "r") as f: + calibration_dict = json.load(f) + else: # Use defaults + calibration_dict = { + "ball_hue": 44, + "pixel_offsets": (0, 0), + "servo_offsets": (0.0, 0.0, 0.0), + } + calibration_dict = validate_calibration(calibration_dict, calibration_file) + return calibration_dict \ No newline at end of file diff --git a/sw/detector.py b/sw/detector.py index f03ed20..86d5c57 100644 --- a/sw/detector.py +++ b/sw/detector.py @@ -12,7 +12,7 @@ from hsv import hue_to_bgr from huemask import hue_mask from typing import List, Optional -from common import Vector2, CircleFeature, Calibration +from common import Vector2, CircleFeature def pixels_to_meters(vec, frame_size=256, field_of_view=1.05): @@ -23,7 +23,7 @@ def pixels_to_meters(vec, frame_size=256, field_of_view=1.05): return np.asarray(vec) * conversion -def meters_to_pixels(vec, frame_size=256, field_of_view=1.0): +def meters_to_pixels(vec, frame_size=256, field_of_view=1.05): # The plate is default roughly 105% of the field of view plate_diameter_meters = 0.225 plate_diameter_pixels = frame_size * field_of_view @@ -49,13 +49,13 @@ def draw_ball(img, center, radius, hue): def draw_crosshairs(img, crosshairs): x, y = crosshairs - lx, ly = img.shape[:2] - cv2.line(img, (0, ly // 2 + y), (ly, ly // 2 + y), (0, 0, 255), 2) # X axis - cv2.line(img, (lx // 2 + x, 0), (lx // 2 + x, lx), (0, 0, 255), 2) # Y axis + ly, lx = img.shape[:2] # Note: y, x + cv2.line(img, (0, ly // 2 + y), (lx, ly // 2 + y), (0, 0, 255), 2) # X axis + cv2.line(img, (lx // 2 + x, 0), (lx // 2 + x, ly), (0, 0, 255), 2) # Y axis return img -def save_img(filepath, img, rotated=False, quality=80): +def save_img(filepath, img, rotated=False, quality=80, mirrored=False): if rotated: # Rotate the image -30 degrees so it looks normal w, h = img.shape[:2] @@ -64,6 +64,10 @@ def save_img(filepath, img, rotated=False, quality=80): img = cv2.warpAffine(img, M, (w, h)) img = img[::-1, :, :] # Mirror along x axis + # Only mirror if the image is not rotated (which does the flip already) + elif mirrored: + img = img[::-1, :, :] # Mirror along x axis + cv2.imwrite( filepath, img, @@ -72,28 +76,23 @@ def save_img(filepath, img, rotated=False, quality=80): def hsv_detector( - calibration=None, frame_size=256, kernel_size=[5, 5], ball_min=0.06, ball_max=0.22, - hue=None, # hue [0..255] + hue=44, # hue [0..360] debug=False, ): - if calibration is None: - calibration = Calibration() - # if we haven't been overridden, use ballHue from calibration - if hue is None: - hue = calibration.ball_hue kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, tuple(kernel_size)) def detect_features( img, hue=hue, + pixel_offsets=(0, 0), debug=debug, filename="/tmp/camera/frame.jpg", crosshairs=None, - axis=None, + axis=False, ): # covert to HSV space img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) @@ -119,6 +118,10 @@ def detect_features( cv2.CHAIN_APPROX_SIMPLE, )[-2] + # Determine the zero point in the image + center_x = img.shape[1] // 2 + pixel_offsets[0] # img.shape[1] is width (x) + center_y = img.shape[0] // 2 + pixel_offsets[1] # img.shape[0] is height (y) + if len(contours) > 0: contour_peak = max(contours, key=cv2.contourArea) ((x_obs, y_obs), radius) = cv2.minEnclosingCircle(contour_peak) @@ -129,24 +132,22 @@ def detect_features( ball_detected = True # Convert from pixels to absolute with 0,0 as center of detected plate - x = int(x_obs - frame_size // 2) - y = int(y_obs - frame_size // 2) + x = x_obs - center_x + y = y_obs - center_y if debug: ball_center_pixels = (int(x_obs), int(y_obs)) draw_ball(img, ball_center_pixels, radius, hue) if crosshairs is not None: draw_crosshairs(img, crosshairs) - if axis is not None: - # Draw the axis (xy plate offsets) - x, y = meters_to_pixels(axis) - x, y = list(Vector2(x, y).rotate(30)) - draw_crosshairs(img, (int(x), int(y))) - save_img(filename, img, rotated=False, quality=80) + if axis: + draw_crosshairs(img, (pixel_offsets[0], pixel_offsets[1])) + save_img(filename, img, rotated=False, quality=80, mirrored=False) # Rotate the x, y coordinates by -30 degrees - center = Vector2(x, y).rotate(np.radians(-30)) - center = pixels_to_meters(center, frame_size) + x, y = pixels_to_meters((x, y)) + center = Vector2(x, y).rotate_deg(-30) + return ball_detected, (center, radius) # If there were no contours or no contours the size of the ball @@ -154,12 +155,9 @@ def detect_features( if debug: if crosshairs is not None: draw_crosshairs(img, crosshairs) - if axis is not None: - # Draw the axis (xy plate offsets) - x, y = meters_to_pixels(axis) - x, y = list(Vector2(x, y).rotate(30)) - draw_crosshairs(img, (int(x), int(y))) - save_img(filename, img, rotated=False, quality=80) + if axis: + draw_crosshairs(img, (pixel_offsets[0], pixel_offsets[1])) + save_img(filename, img, rotated=False, quality=80, mirrored=False) return ball_detected, (Vector2(0, 0), 0.0) return detect_features diff --git a/sw/hardware.py b/sw/hardware.py index e1aec61..8033224 100644 --- a/sw/hardware.py +++ b/sw/hardware.py @@ -6,11 +6,11 @@ import json import numpy as np -from common import Vector2 -from detector import hsv_detector -from typing import Tuple, Optional -from camera import OpenCVCameraSensor +from detector import hsv_detector, meters_to_pixels from hat import Hat, Buttons, Icon, PowerIcon +from common import Vector2, read_calibration +from camera import OpenCVCameraSensor +from typing import Tuple, Optional def plate_angles_to_servo_positions( @@ -80,24 +80,14 @@ def __repr__(self): return self.__str__() def __str__(self): - return f"hue: {self.hue}, servo offsets: {self.servo_offsets}, plate offsets: {self.plate_offsets}" + return f"hue: {self.hue}, servo offsets: {self.servo_offsets}, pixel offsets: {self.pixel_offsets}" def reset_calibration(self, calibration_file=None): - # Use default if not defined + # Use default file if not defined calibration_file = calibration_file or self.calibration_file - # Get calibration settings - if os.path.isfile(calibration_file): - with open(calibration_file, "r") as f: - calib = json.load(f) - else: # Use defaults - calib = { - "ball_hue": 44, # Hue - "plate_offsets": (0.0, 0.0), # X,Y offset in meters - "servo_offsets": (0.0, 0.0, 0.0), # Servo angle offset in degrees - } - - self.plate_offsets = calib["plate_offsets"] + calib = read_calibration(calibration_file) + self.pixel_offsets = calib["pixel_offsets"] self.servo_offsets = calib["servo_offsets"] self.hue = calib["ball_hue"] @@ -156,11 +146,8 @@ def step(self, pitch, roll) -> Buttons: buttons = self.hat.get_buttons() detected, (center, radius) = self.detector( - frame, hue=self.hue, axis=self.plate_offsets + frame, hue=self.hue, pixel_offsets=self.pixel_offsets, axis=True ) - center.x -= self.plate_offsets[0] - center.y -= self.plate_offsets[1] - # Return the ball center, ball betected, and buttons return center, detected, buttons \ No newline at end of file