-
Notifications
You must be signed in to change notification settings - Fork 0
/
Track Target with Camera Main.py
127 lines (98 loc) · 4.15 KB
/
Track Target with Camera 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
# -*- coding: utf-8 -*-
"""
Created on Fri May 13 18:10:44 2016
@author: Ithier
"""
import FindTargetModule as FTM
import NetworkTableModule as NT
import allowableError as AE
import time
import cv2
directory = 'C:/Users/Ithier/Documents/OpenCV/FIRST 2016/PowerKnightsVision2016/'
#directory = 'C:/Users/Ithier/Documents/!!OpenCV/Mayhem/' # folder npz file is in
frame_0 = directory + 'Raw/'
frame_p = directory + 'Processed/'
filename = directory + 'imageValues.npz'
url = 'http://10.5.1.11/axis-cgi/mjpg/video.cgi?resolution=640x480'
freqFramesCam = 20 # how often we're sampling the camera to save files
freqFramesNT = 10 # how often we're sending to network tables
validCount = 0 # initialize how many validUpdates we've had
#############################################################################
from networktables import NetworkTable
import logging
if NetworkTable._staticProvider is None:
logging.basicConfig(level=logging.DEBUG)
NetworkTable.setIPAddress('10.5.1.2')
NetworkTable.setClientMode()
NetworkTable.initialize()
sd = NetworkTable.getTable("Camera")
##############################################################################
cap = cv2.VideoCapture(url) # capture camera, 0 is laptop cam, numbers after that are cameras attached
# Check to make sure cap was initialized in capture
if cap.isOpened():
print 'Cap succesfully opened'
print cap.grab()
else:
print 'Cap initialization failed'
# Create resizable window for camera
cv2.namedWindow('Camera Frame', cv2.WINDOW_NORMAL)
c = freqFramesCam
n = freqFramesNT
while(cap.isOpened()):
# Capture frame-by-frame
# ret returns true or false (T if img read correctly); frame is array of img
ret, frame = cap.read()
if frame is None:
print 'Frame is None'
Processed_frame = cv2.imread('1.png', 1)
mask = Processed_frame
else:
try:
# Save frame to file
if c > freqFramesCam:
# Determine time stamp
t = time.localtime()
stamp = str(t[1]) + "_" + str(t[2]) + "_" + str(t[0]) + "time_" + str(t[3]) + "_" + str(t[4]) + "_" + str(t[5]) + "_" + str(n)
cv2.imwrite(frame_0 + stamp + '.jpg', frame)
c = 0
else:
c = c + 1
# Rotate image
rows, cols, dim = frame.shape
M = cv2.getRotationMatrix2D((cols/2, rows/2), 90, 1) # image center, angle, scaling factor
Rotated_frame = cv2.warpAffine(frame,M,(cols,rows))
# Process image
Angle, Distance, validUpdate, Processed_frame, mask = FTM.findTarget(Rotated_frame,filename)
Locked = False
# Update how many validUpdates we've had so far
if validUpdate: validCount += 1
# Determine allowable angle offset
Error = AE.findError(18)
#Error = AE.findError(Distance)
# If anle is in acceptable range, write LOCKED on picture
if -Error <= Angle <= Error:
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(Processed_frame,'LOCKED',(90,400), font, 3.8,(0,255,0),6,cv2.LINE_AA)
Locked = True
if n > freqFramesNT:
# Send to NetworkTable
NT.sendValues(sd, Angle, Distance, validCount, Locked)
n = 0
else:
n = n + 1
# Put crosshairs on image
rows, cols, dim = Processed_frame.shape
cv2.line(Processed_frame, (cols/2,0), (cols/2,rows), (255,0,0), 2)
cv2.line(Processed_frame, (0,rows/2), (cols,rows/2), (255,0,0), 2)
# Save processed frame
cv2.imwrite(frame_p + stamp + '.jpg', Processed_frame)
except:
print 'There was an error'
# Display the resulting frame
cv2.imshow('Camera Frame', Processed_frame)
cv2.imshow('Mask', mask)
if cv2.waitKey(5) & 0xFF == ord('q'):
break
# When capture done, release it
cap.release() # !! important to do
cv2.destroyAllWindows()