Robot2017-Vision/image.py at master · team467/Robot2017-Vision · GitHub

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
from grip import GripPipeline
p = GripPipeline()

import cv2
import os
import math

FOVTangent = math.tan(math.radians(26))
focalLength = 320/FOVTangent

def initCameras():
	os.system("v4l2-ctl -c exposure_auto=1 -c exposure_absolute=5")
	global capture
	global capture1
	capture = cv2.VideoCapture(0)
	capture1 = cv2.VideoCapture(1)
	capture.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
	capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
	capture1.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
	capture1.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

capture = None
capture1 = None
initCameras()
res, image = capture.read()
res, image = capture.read()
res, image = capture.read()
res, image = capture.read()
p.process(image)
cv2.imwrite('image-raw.jpg', image)
cv2.drawContours(image, p.filter_contours_output, -1, (0,255,0), 3)
for contour in p.filter_contours_output:
	x,y,w,h = cv2.boundingRect(contour)
	cv2.rectangle(image,(x,y),(x+w,y+h),(0,0,255),2)
	center = x + w/2
	angle = math.degrees(math.atan((center - 320) / focalLength))
	print(angle)
print(p.filter_contours_output)
cv2.imwrite('image-contours.jpg', image)
res, image = capture1.read()
cv2.imwrite('image-capture1.jpg', image)