def __init__(self, cam_idx = 0):

if camera == 'pi':

# modprobe required after pi reboot

os.system('sudo modprobe bcm2835-v4l2') # Load the BCM V4l2 driver for /dev/video0

os.system('v4l2-ctl -p 4') # Set the framerate (not sure this does anything!)

#os.system('v4l2-ctl -c focus_auto=0') # Disable autofocus??

# try 'v4l2-ctl -l' to show all possible controls

pass

self.cam_idx = cam_idx

def start(self):

self.capture = cv2.VideoCapture(self.cam_idx)

# self.capture.set(cv2.CAP_PROP_SETTINGS, 1) # pops up dialog for webcam settings

self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, capsize.x)

self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, capsize.y)

buffsize = self.capture.get(cv2.CAP_PROP_BUFFERSIZE)

if buffsize != -1:

print("VideoCapture buffer size = %d, setting to 1" % buffsize)

self.capture.set(cv2.CAP_PROP_BUFFERSIZE, 1)

# Start new thread to continuously get images

time.sleep(2 if camera == 'pi' else 1)

self.quit = False

self.ret, self.currentFrame = self.capture.read()

self.img_thread = Thread(target=self.updateFrame, args=()).start()

def stop(self):

self.quit = True

self.img_thread.join()

self.currentFrame = None

self.capture.release()

# Continually updates self.currentFrame

def updateFrame(self):

while not self.quit:

ret, frame = self.capture.read()

while ret == False: # Continually grab frames until we get a good one

time.sleep(0.005) # but sleep for 5 milliseconds

ret, frame = self.capture.read()

self.ret = True

self.currentFrame = frame

def getFrame(self):

return (self.ret, self.currentFrame)

def nextFrame(self):

self.currentFrame = None

global faceCascade

if sys.platform == 'win32':

cascPath = 'C:/greg/dev/opencv/data/lbpcascades/lbpcascade_frontalface.xml'

else:

cascPath = '/usr/local/share/OpenCV/lbpcascades/lbpcascade_frontalface.xml'

global faceCascade

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

gray = cv2.equalizeHist( gray )

faces = faceCascade.detectMultiScale(gray, 1.1, 3, 0, (10, 10))

if show_image:

for (x, y, w, h) in faces:

cv2.rectangle(gray, (x, y), (x+w, y+h), (0, 255, 0), 2) # Draw a green rectangle around the face

for (x, y, w, h) in faces:

return (gray, point(x, y) + (point(w, h) / 2)) # and return the center of the first one found

# Convert to greyscale for detection

# ----------------------------------

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

gray = cv2.equalizeHist(gray)

global cam_center_angle

global cam_pan_cur, cam_tilt_cur

# Correct relative to center of image

# -----------------------------------

diff = p - capsize / 2

# Convert to percentage offset

# ----------------------------

diff.toFloat()

if feedback:

print('diff = (%f, %f)' % (diff.x, diff.y)) # in pixels

turn = point(diff.x / capsize.x, diff.y / capsize.y) # in %

## servo seems to react with excessively large movement to tilt commands, so

## divide turn.y by 8 instead of by 2

turn = point(turn.x / 2.0, turn.y / 8)

if feedback:

print('turn = (%f, %f)' % (turn.x, turn.y))

# Scale offset to degrees

# -----------------------

turn.x = turn.x * fov.x

turn.y = turn.y * fov.y

# print('turn = (%f, %f)' % (turn.x, turn.y))

cam_pan = -turn.x

cam_tilt = cam_center_angle + turn.y

# Update the robot

# ----------------

if feedback:

print('pan = %f, tilt = %f ' % (cam_pan, cam_tilt))

if use_servo:

cam_pan_cur += cam_pan

cam_tilt_cur += cam_tilt

if feedback:

print('pan_cur = %f, tilt_cur = %f ' % (cam_pan_cur, cam_tilt_cur))

# Clamp Pan/Tilt to 0 to 180 degrees

# ----------------------------------

cam_tilt_cur = max(0, min(130, cam_tilt_cur))

pan(cam_pan_cur)

tilt(cam_tilt_cur)

else:

if int(cam_pan) == 0:

raspi.putNumber('shoot', 1)

else:

raspi.putNumber('shoot', 0)

raspi.putNumber('pan', int(cam_pan))

logging.basicConfig(level=logging.DEBUG) # to see messages from networktables

NetworkTable.setIPAddress('127.0.0.1')

# NetworkTable.setIPAddress('roborio-5260.local')

NetworkTable.setClientMode()

NetworkTable.initialize()

cam_stream = CameraStream(0)

cam_stream.start()

if use_servo:

pan(cam_pan_cur) # Turn the camera to the default position

tilt(cam_tilt_cur)

raspi = None

else:

raspi = initNetworktables()

if method == 0:

initFaceTracking()

elif method == 1:

# Initiate SIFT detector

sift = cv2.xfeatures2d.SIFT_create()

# find the keypoints and descriptors with SIFT

img1 = cv2.imread('target.png', 0)

if img1 and not img1.empty():

kp1, desc1 = sift.detectAndCompute(img1, None)

#skip = 0

loop_cnt = 0

while True:

loop_cnt += 1

ret, frame = cam_stream.getFrame() # get a frame

if ret == False:

if loop_cnt < 10 or loop_cnt % 100 == 0:

print("Error getting image") # if we didn't get a frame, try again

time.sleep(0.025) # sleep for 25 milliseconds so the image thread can get its image

continue # maybe it was just a hiccup!

#if skip:

# skip = skip - 1

#if skip:

# continue

if method == 0:

img, p = findFace(frame) # Do face detection

elif method == 1 and img1 and not img1.empty():

img, p = findTargetSift(frame, kp1, descl) # Do target detection using SIFT

else:

img, p = None, None

if p:

#cv2.circle(frame, p.asTuple(), 3, 255, -1)

track(p, raspi) # Point the camera to the returned position

#skip = 5

# only on windows, show captured image in window

# ----------------------------------------------

if show_image:

frame = img

frame = cv2.resize(frame, (capsize * 1).asTuple())

#frame = cv2.flip(frame, 1)

cv2.imshow('Video', frame) # Display the image, with rectangle

if p:

cam_stream.nextFrame() # make sure we get a frame after the servo moved

time.sleep(0.030) # sleep for a few milliseconds

if cv2.waitKey(1) & 0xFF == ord('q'):

break

# When everything is done, release the capture

# --------------------------------------------

cam_stream.stop()