-
Notifications
You must be signed in to change notification settings - Fork 0
/
cam.py
103 lines (97 loc) · 4.35 KB
/
cam.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
#!/usr/bin/env python3
#----------------------------------------------------------------------------
# Copyright (c) 2018 FIRST. All Rights Reserved.
# Open Source Software - may be modified and shared by FRC teams. The code
# must be accompanied by the FIRST BSD license file in the root directory of
# the project.
#----------------------------------------------------------------------------
import time
import sys
import cv2
import numpy as np
from cscore import CameraServer, VideoSource
from networktables import NetworkTablesInstance
if __name__ == "__main__":
#Start network tables to conect to dashboard
ntinst = NetworkTablesInstance.getDefault()
print("Setting up NetworkTables client for team {}".format(5800))
ntinst.initialize(server='10.58.0.2')
sd = ntinst.getTable("SmartDashboard")
sd.putNumber("SomeNumber", 1234)
# Get CameraServer and initialize camera(s)
print("Setting up cameras")
cs = CameraServer.getInstance()
# Start camera 0
cam0 = cs.startAutomaticCapture(name="cam0", path='/dev/video0')
cam0.setResolution(160, 120)
#Start camera 1
cam1 = cs.startAutomaticCapture(name="cam1", path='/dev/video1')
cam1.setResolution(160,120)
# Get a CvSink. This will capture images from the camera
cvSink = cs.getVideo(name="cam0")
cvSink1 = cs.getVideo(name="cam1")
# (optional) Setup a CvSource. This will send images back to the Dashboard
outputStream = cs.putVideo(name="p_cam0", width=160, height=120)
outputStream1 = cs.putVideo(name="p_cam1", width=160, height=120)
# sending cam to dashboard
cameras = []
cameras.append(cam0)
cameras.append(outputStream)
cameras.append(cam1)
cameras.append(outputStream1)
# Allocating new images is very expensive, always try to preallocate
img = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
img1 = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
hsv = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
mask = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
out = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
blur = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
closing = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
opening = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
dilation = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
erosion = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
edges = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
contours = np.zeros(shape=(160, 120, 3), dtype=np.uint8)
kernel3 = np.ones((3,3), np.uint8)
kernel9 = np.ones((11,11), np.float32)/225
# Range setting
lower_led = np.array([0, 150, 60])
upper_led = np.array([13, 240, 170])
# loop forever
while True:
# Tell the CvSink to grab a frame from the camera and put it
# in the source image. If there is an error notify the output.
time, img = cvSink.grabFrame(img)
time, img1 = cvSink.grabFrame(img1)
if time == 0:
# Send the output the error.
outputStream.notifyError(cvSink.getError());
outputStream1.notifyError(cvSink.getError());
# skip the rest of the current iteration
continue
# Put your openCv logic here
hsv = cv2.cvtColor(img1, cv2.COLOR_BGR2HSV)
# Mask Applying
mask = cv2.inRange(hsv, lower_led, upper_led)
#Image filtering
blur = cv2.medianBlur(mask, 15)
closing = cv2.morphologyEx(blur, cv2.MORPH_CLOSE, kernel9)
opening = cv2.morphologyEx(closing, cv2.MORPH_OPEN, kernel9)
dilation = cv2.dilate(opening, kernel9, iterations = 1)
erosion = cv2.erode(dilation, kernel3, iterations = 1)
# Edge detection
edges = cv2.Canny(erosion, 100, 150)
erosion, contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
#Center of the object
if len(contours) != 0:
c = max(contours, key = cv2.contourArea)
x, y, w, z = cv2.boundingRect(c)
cy = int(y+(z/2))
cx = int(x+(w/2))
#coordinates = ('X' + str(cx) + 'Y' + str(cy) + 'N' + str(n)) #String form
#print(coordinates)
cv2.circle(erosion, (cx, cy), 5, (150, 0, 255), -1)
#sd.putNumber("Coordinate X", cx)
#sd.putNumber("Coordinate Y", cy)
outputStream1.putFrame(erosion)
outputStream.putFrame(img)