Added tests, changed detection code, minor changes

cameraServer.py

@@ -7,22 +7,21 @@
 import time
 from collections import namedtuple
 from cscore import CameraServer
-from networktables import NetworkTables
-
+from networktables import NetworkTablesInstance, NetworkTables
 
 # Magic Numbers
-lowerGreen = (50, 120, 130)  # Our Robot's Camera
-higherGreen = (100, 220, 220)
-minContourArea = 10
+lowerGreen = (70, 130, 90)  # Our Robot's Camera
+higherGreen = (85, 255, 205)
+minContourArea = 5
 angleOffset = 10
-rightAngleSize = -14
+rightAngleSize = -14.5
 leftAngleSize = -75.5
 screenX = 320
 screenY = 240
 screenSize = (screenX, screenY)
-distance_away = 110
 realTapeDistance = 0.2  # metres between closest tape points
-focal_length = 325
+focal_length = 325 * screenX / 320
+areaRatio = 0.5
 
 # Initialisation
 configFile = "/boot/frc.json"
@@ -72,35 +71,30 @@ def getDistance(boxes):
         dist = (realTapeDistance * focal_length) / width
         return dist, offset
     else:
-        return math.nan, offset
+        return math.nan, math.nan
 
 
 def getOffset(width, x):
-    # if width = 20cm then what is x in cm
     offset = x / (width / (realTapeDistance))
     return -offset
 
 
 def createAnnotatedDisplay(
-    frame: np.array, pairs: list, closestToMiddle: tuple, circle: tuple
+    frame: np.array, circle: tuple
 ) -> np.array:
-    frame = cv2.line(frame, (160, 0), (160, 240), (255, 0, 0), thickness=1)
-    for pair in pairs:
-        if (pair[0][1][0] == closestToMiddle[0][0]).all():
-            colour = (0, 255, 0) #Green
-            frame = cv2.circle(
-                frame, (int(circle[0][0]), int(circle[0][1])), int(circle[1]), colour
-            )
-        else:
-            colour = (0, 0, 255) #Red
-        for tape in pair:
-            frame = cv2.drawContours(
-                frame, [np.int0(tape[1])], 0, colour, thickness=2
-            )
+    frame = cv2.circle(
+        frame,
+        (int(circle[0][0]), int(circle[0][1])),
+        int(circle[1] * 1.3),
+        (0, 0, 255),
+        thickness=1,
+    )
     return frame
 
 
-def getRetroPos(frame: np.array, annotated: bool, hsv: np.array, mask: np.array) -> (np.array, float, float):
+def getRetroPos(
+    frame: np.array, annotated: bool, hsv: np.array, mask: np.array
+) -> (np.array, float, float):
     """Function for finding retro-reflective tape"""
 
     hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV, dst=hsv)
@@ -122,40 +116,57 @@ def getRetroPos(frame: np.array, annotated: bool, hsv: np.array, mask: np.array)
     boxed_and_angles = []
     for rect in rects:
         if math.isclose(rect[2], leftAngleSize, abs_tol=angleOffset):
-            boxed_and_angles.append([False, np.array(cv2.boxPoints(rect)), cv2.contourArea(cv2.boxPoints(rect))])
+            boxed_and_angles.append(
+                [
+                    False,
+                    np.array(cv2.boxPoints(rect)),
+                    cv2.contourArea(cv2.boxPoints(rect)),
+                    max(rect[1]),
+                ]
+            )
         elif math.isclose(rect[2], rightAngleSize, abs_tol=angleOffset):
-            boxed_and_angles.append([True, np.array(cv2.boxPoints(rect)), cv2.contourArea(cv2.boxPoints(rect))])
+            boxed_and_angles.append(
+                [
+                    True,
+                    np.array(cv2.boxPoints(rect)),
+                    cv2.contourArea(cv2.boxPoints(rect)),
+                    max(rect[1]),
+                ]
+            )
 
     pairs = []
-    leftRect = None
+    leftRects = []
     for rect in sorted(
         boxed_and_angles, key=lambda x: max(x[1][:, 0]) if x[0] else min(x[1][:, 0])
     ):  # Get rectangle pairs
         if not rect[0]:
-            leftRect = rect
-        elif leftRect and math.isclose(leftRect[2], rect[2], abs_tol=0.3*leftRect[2]):
-            pairs.append((leftRect, rect))
-            leftRect = None
-
+            leftRects.append(rect)
+        elif len(leftRects) > 0:
+            for leftRect in leftRects:
+                if math.isclose(leftRect[2], rect[2], rel_tol=areaRatio) and math.isclose(rect[3] * 1.5, min(rect[1][:, 0]) - max(leftRect[1][:, 0]), rel_tol=0.5):
+                    pairs.append((leftRect, rect))
+                    leftRects = []
+                    break
+        else:
+            pass
+        frame = cv2.drawContours(frame, np.int0([rect[1]]), 0, (255, 0, 255))
     if len(pairs) < 1:
         return frame, math.nan, math.nan
-
-    closestToMiddle = list(min(
-        pairs, key=lambda x: abs(np.mean([x[0][1][:,0] + x[1][1][:,0]]) - screenSize[0])
-    ))
+    closestToMiddle = list(
+        min(
+            pairs,
+            key=lambda x: abs(np.mean([x[0][1][:,0] + x[1][1][:,0]]) - screenSize[0]),
+        )
+    )
     closestToMiddle = [closestToMiddle[0][1], closestToMiddle[1][1]]
 
     (x, y), radius = cv2.minEnclosingCircle(np.array(closestToMiddle).reshape(-1, 2))
 
     if annotated:
-        frame = createAnnotatedDisplay(frame, pairs, closestToMiddle, ((x, y), radius))
+        frame = createAnnotatedDisplay(frame, ((x, y), radius))
 
     dist, offset = getDistance(closestToMiddle)
-    return (
-        frame,
-        dist,
-        offset,
-    )
+    return (frame, dist, offset)
 
 
 if __name__ == "__main__":
@@ -166,19 +177,23 @@ def getRetroPos(frame: np.array, annotated: bool, hsv: np.array, mask: np.array)
     cameraConfigs = readConfig()
 
     # start NetworkTables
-    NetworkTables.initialize(server="10.47.74.2")
 
+    ntinst = NetworkTablesInstance()
+    ntinst.startServer()
+    ntinst.setUpdateRate(1)
+
+    NetworkTables.initialize(server="10.47.74.2")
     NetworkTables.setUpdateRate(1)
-    nt = NetworkTables.getTable("/vision")
-    ping = nt.getEntry("ping")
-    raspi_pong = nt.getEntry("raspi_pong")
-    rio_pong = nt.getEntry("rio_pong")
 
-    entry_game_piece = nt.getEntry("game_piece")
-    entry_dist = nt.getEntry("fiducial_x")
-    entry_offset = nt.getEntry("fiducial_y")
-    entry_fiducial_time = nt.getEntry("fiducial_time")
-    entry_camera = nt.getEntry("using_cargo_camera")
+    ping = ntinst.getEntry("/vision/ping")
+    raspi_pong = ntinst.getEntry("/vision/raspi_pong")
+    rio_pong = ntinst.getEntry("/vision/rio_pong")
+
+    entry_game_piece = ntinst.getEntry("/vision/game_piece")
+    entry_dist = ntinst.getEntry("/vision/fiducial_x")
+    entry_offset = ntinst.getEntry("/vision/fiducial_y")
+    entry_fiducial_time = ntinst.getEntry("/vision/fiducial_time")
+    entry_camera = ntinst.getEntry("/vision/using_cargo_camera")
 
     # start cameras
     cameras = []
@@ -194,18 +209,17 @@ def getRetroPos(frame: np.array, annotated: bool, hsv: np.array, mask: np.array)
     hsv = np.zeros(shape=(screenSize[1], screenSize[0], 3), dtype=np.uint8)
     mask = np.zeros(shape=(screenSize[1], screenSize[0]), dtype=np.uint8)
     img = np.zeros(shape=(screenSize[1], screenSize[0], 3), dtype=np.uint8)
-
     old_ping_time = 0
     while True:
         ping_time = ping.getNumber(0)
         if abs(ping_time - old_ping_time) > 0.00000001:
             raspi_pong.setNumber(time.monotonic())
             rio_pong.setNumber(ping_time)
             old_ping_time = ping_time
-        game_piece = entry_game_piece.getBoolean(0)
+        game_piece = entry_game_piece.getNumber(0)
         fiducial_time = time.monotonic()
         sink = hatch_sink if game_piece == 0 else cargo_rocket_sink
-        entry_camera.setBoolean(False if not game_piece else True)
+        entry_camera.setBoolean(bool(game_piece))
         frame_time, frame = sink.grabFrameNoTimeout(image=frame)
         if frame_time == 0:
             print(sink.getError(), file=sys.stderr)
@@ -215,11 +229,16 @@ def getRetroPos(frame: np.array, annotated: bool, hsv: np.array, mask: np.array)
         else:
             image, dist, offset = getRetroPos(frame, True, hsv, mask)
         source.putFrame(image)
-        if not math.isnan(dist):
+        if (
+            not math.isnan(dist)
+            and not dist < 0.6
+            and not dist > 3
+            and not abs(offset) > 2
+        ):
             if game_piece == 1:
                 dist *= -1
                 offset *= -1
             entry_dist.setNumber(dist)
             entry_offset.setNumber(offset)
             entry_fiducial_time.setNumber(fiducial_time)
-        NetworkTables.flush()
+        ntinst.flush()

samples/target.csv

@@ -0,0 +1,7 @@
+350mm.png,0.35,0
+530mm.png,0.53,0
+740mm.png,0.74,0
+830-300mm.png,0.83,-0.3
+860-90mm.png,0.86,-0.09
+1210mm.png,1.21,0
+nothing.png,nan,nan

tests.py

@@ -0,0 +1,53 @@
+from cameraServer import getRetroPos
+import cv2
+import numpy as np
+import math
+import csv
+import sys
+
+
+def test_images():
+    acceptable_error = 0.08
+    with open("samples/target.csv", "r") as csvfile:
+        csvreader = csv.reader(csvfile, delimiter=",")
+        csvlist = list(csvreader)
+        for row in csvlist:
+            if row:
+                frame = cv2.imread("samples/" + row[0])
+                result = getRetroPos(frame, True, hsv, mask)
+                data = (float(row[1]), float(row[2]))  # Distance, Offset
+                if math.isnan(result[1]):
+                    assert math.isnan(data[0]) and math.isnan(result[1])
+                    assert math.isnan(data[1]) and math.isnan(result[2])
+                    continue
+                assert (
+                    data[0] - acceptable_error < result[1] < data[0] + acceptable_error
+                )
+                assert (
+                    data[1] - acceptable_error < result[2] < data[1] + acceptable_error
+                )
+
+
+def test_video(path):
+    cap = cv2.VideoCapture(path)
+    if cap.isOpened() == False:
+        print("Error opening video stream or file", file=sys.stderr)
+        sys.exit()
+    for _ in range(2350):
+        _, _ = cap.read()
+    while True:
+        ret, frame = cap.read()
+        result = getRetroPos(frame, True, hsv, mask)
+        if ret:
+            cv2.imshow("frame", result[0])
+            print(result[1], result[2])
+            cv2.waitKey(0)
+        else:
+            break
+
+if __name__ == "__main__":
+    screenSize = (320, 240)
+    hsv = np.zeros(shape=(screenSize[1], screenSize[0], 3), dtype=np.uint8)
+    mask = np.zeros(shape=(screenSize[1], screenSize[0]), dtype=np.uint8)
+    test_images()
+