-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.py
150 lines (119 loc) · 4.38 KB
/
main.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
#!/usr/bin/env python3
import numpy
import time
import threading, multiprocessing
import cv2
from networktables import NetworkTables
import cscore
active = True
camera1 = cv2.VideoCapture(0)
#camera2 = cv2.VideoCapture("http://axis-camera.local")
ballx, bally, ballw, ballh, ballarea = 0, 0, 0, 0, 0
reflectorx, reflectory, reflectorw, reflectorh, reflectorarea = 0, 0, 0, 0, 0
ballfound = False
reflectorfound = False
NetworkTables.initialize(server='roborio-3405-frc.local')
table = NetworkTables.getTable('SmartDashboard')
cs = cscore.CameraServer.getInstance()
videoout1 = cs.putVideo('camera1', 640, 480)
videoout2 = cs.putVideo('camera2', 640, 480)
shadeout1 = cs.putVideo('mask1', 180, 135)
shadeout2 = cs.putVideo('mask2', 180, 135)
size = (1920, 1080)
def getkey(item):
return item[4]
def outputvideo():
while active:
ret1, frame1 = camera1.read()
ret2 = False
#ret2, frame2 = camera2.read()
if ret1:
frame1 = cv2.resize(frame1, (320, 240))
videoout1.putFrame(frame1)
if ret2:
frame2 = cv2.resize(frame2, (320, 240))
videoout2.putFrame(frame2)
time.sleep(.03)
def updater():
while active:
table.putNumber('ballx', ballx - size[0]/2)
table.putNumber('bally', bally - size[1]/2)
table.putNumber('ballw', ballw)
table.putNumber('ballh', ballh)
table.putNumber('ballarea', ballarea)
table.putBoolean('ballfound', ballfound)
table.putNumber('reflectorx', reflectorx - size[0]/2)
table.putNumber('reflectory', reflectory - size[1]/2)
table.putNumber('reflectorw', reflectorw)
table.putNumber('reflectorh', reflectorh)
table.putNumber('reflectorarea', reflectorarea)
table.putBoolean('reflectorfound', reflectorfound)
time.sleep(.03)
# game piece offset
def gamePieceOffset(camera):
upmask = numpy.array([20, 125, 255])
downmask = numpy.array([0, 50, 150])
ret, frame = camera.read()
if not ret:
return [False]
frame = cv2.resize(frame, size)
mask = cv2.inRange(frame, downmask, upmask)
mask1 = cv2.resize(mask, (180, 135))
shadeout1.putFrame(mask1)
rand1, contours, rand2 = cv2.findContours(mask, mode=cv2.RETR_LIST, method=cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
contours.sort(key=lambda contour: cv2.contourArea(contour))
ballx, bally, ballw, ballh = cv2.boundingRect(contours[0])
ballarea = cv2.contourArea(contours[0])
return [True, ballx, bally, ballw, ballh, ballarea]
else:
return [False]
# hatch panel tape offset
def reflectorFinder(camera):
upmask = numpy.array([102, 255, 255])
downmask = numpy.array([73, 161, 138])
ret, frame = camera.read()
if not ret:
return [False]
frame = cv2.resize(frame, size)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, downmask, upmask)
mask1 = cv2.resize(mask, (180, 135))
shadeout2.putFrame(mask1)
rand3, contours, rand4 = cv2.findContours(mask, mode=cv2.RETR_LIST, method=cv2.CHAIN_APPROX_SIMPLE)
goodcontours = []
for contour in contours:
# variables
x, y, w, h = cv2.boundingRect(contour)
area = cv2.contourArea(contour)
ratio = float(w/h)
# test
if area > 20 and ratio < .7:
goodcontours.append([x, y, w, h, area])
goodcontours.sort(key=getkey)
if len(goodcontours) > 1:
reflectorx = (goodcontours[0][0] + goodcontours[1][0])/2
reflectory = (goodcontours[0][1] + goodcontours[1][1])/2
reflectorw, reflectorh, reflectorarea = goodcontours[0][2:]
return [True, reflectorx, reflectory, reflectorw, reflectorh, reflectorarea]
else:
return [False]
# init
updaterthread = threading.Thread(target=updater, name="updater")
updaterthread.start()
outputvideothread = threading.Thread(target=outputvideo, name="outputvideo")
outputvideothread.start()
while active:
ball = gamePieceOffset(camera1)
#reflector = reflectorFinder(camera2)
if ball[0]:
ballx, bally, ballw, ballh, ballarea = ball[1:]
ballfound = True
else:
ballfound = False
'''if reflector[0]:
reflectorx, reflectory, reflectorw, reflectorh, reflectorarea = reflector[1:]
reflectorfound = True
else:
reflectorfound = False'''
time.sleep(1)