You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

128 lines
4.2 KiB
Python

#!/usr/bin/env python
from __future__ import print_function
import numpy as np
import cv2, math, sys
# import video
from picamera.array import PiRGBArray
from picamera import PiCamera
import OSC
# from pythonosc import osc_message_builder
# from pythonosc import udp_client
import time
# MESSAGES NEED TO GO TO STDERR
print ("1.HELLO FROM PYTHON stderr", file=sys.stderr)
# def draw_flow(img, flow, step=4): # size grid
# h, w = img.shape[:2]
# y, x = np.mgrid[step/2:h:step, step/2:w:step].reshape(2,-1)
# fx, fy = flow[y,x].T
# lines = np.vstack([x, y, x+fx, y+fy]).T.reshape(-1, 2, 2)
# lines = np.int32(lines + 0.5)
# vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
# cv2.polylines(vis, lines, 0, (0, 0, 255)) # BGR
# for (x1, y1), (x2, y2) in lines:
# cv2.circle(vis, (x1, y1), 1, (0, 255, 0), -1)
# return vis
def send_flow0(img, flow, step=4): # size grid
h, w = img.shape[:2]
y, x = np.mgrid[step/2:h:step, step/2:w:step].reshape(2,-1)
fx, fy = flow[y,x].T
#print "fx, fy", fx, fy
lines = np.vstack([x, y, x+fx, y+fy]).T.reshape(-1, 2, 2)
lines = np.int32(lines + 0.5)
vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
flines = []
for (x1, y1), (x2, y2) in lines:
# print ("y1", y1)
if (x1 == 38 or x1 == 46 or x1 == 54 or x1 == 62 or x1 == 70 or x1 == 78 or x1 == 86 or x1 == 94 or x1 == 102 or x1 == 110 or x1 == 118) and y1 in range(38, 90, 8):
flines.append(((x1,y1),(x2,y2)))
normx = x1 / 8 - 4
normy = 1 - ((y1 / 8 - 4) / 3.0)
dx = x2-x1
dy = y2 - y1
m = int(math.sqrt( (dx*dx) + (dy*dy) ))
if m>2:
print ("dot", (normx, normy), file=sys.stderr)
msg = OSC.OSCMessage()
msg.setAddress("/dot")
#msg.append(dx)
#msg.append(dy)
#msg.append(m)
msg.append(normx)
msg.append(normy)
try:
client.send(msg)
except OSC.OSCClientError:
print ("Unable to send OSC", file=sys.stderr)
# client.send_message("/franc", m)
# flines = np.int32(flines)
# cv2.polylines(vis, flines, 0, (0, 40, 255)) # BGR
# for (x1, y1), (x2, y2) in flines:
# cv2.circle(vis, (x1, y1), 1, (0, 255, 0), -1)
# return vis
# flines = np.int32(flines)
# cv2.polylines(vis, flines, 0, (0, 40, 255)) # BGR
# for (x1, y1), (x2, y2) in flines:
# cv2.circle(vis, (x1, y1), 1, (0, 255, 0), -1)
# return vis
# cv2.rectangle(img, pt1, pt2, color[, thickness[, lineType[, shift]]])
if __name__ == '__main__':
#data="hello"
# client = udp_client.SimpleUDPClient("127.0.0.1", 9001)
# connect camera
# cam = video.create_capture("0:size=160x120") #canvas size in pixels
print ("Starting camera", file=sys.stderr)
cam = PiCamera()
framesize = (160, 128)
cam.resolution = framesize
cam.framerate = 32
rawCapture = PiRGBArray(cam, size=framesize)
# allow the camera to warmup
time.sleep(0.25)
print ("Starting main camera loop", file=sys.stderr)
# connect to pd
# Init OSC
while True:
try:
client = OSC.OSCClient()
client.connect(('127.0.0.1', 9001)) # first argument is the IP of the host, second argument is the port to use
break
except OSC.OSCClientError:
print ("Unable to connect via OSC to pd, trying again in 5", file=sys.stderr)
time.sleep(5)
prevgray = None
for frame in cam.capture_continuous(rawCapture, format="bgr", use_video_port=True):
# while True:
# print "GRAB FRAME"
img = frame.array
# ret, img = cam.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
if prevgray != None:
flow = cv2.calcOpticalFlowFarneback(prevgray, gray, 0.5, 3, 15, 3, 5, 1.2, 0)
send_flow0(gray, flow)
prevgray = gray
# clear the stream in preparation for the next frame
rawCapture.truncate(0)
# cv2.imshow('flow', )
#ch = 0xFF & cv2.waitKey(5)
#if ch == 27:
# break
# cv2.destroyAllWindows()