def detect_green_blob( img_a, step=5, avg_rad=3 ):
img = utils.array2cv(img_a)
#h, w, c = img_a.shape
h = img.height
w = img.width
c = 3
TOP, BOTTOM, LEFT, RIGHT = h,-1,w,-1
step, avg_rad = int(step), int(avg_rad)
y = 0
while y < h:
x = 0
while x < w:
if _is_green(img, x, y, avg_rad):
TOP = min(TOP, y)
BOTTOM = max(BOTTOM, y)
LEFT = min(LEFT, x)
RIGHT = max(RIGHT, x)
x += step
y += step
#print "---top,bottom,left,right", TOP, BOTTOM, LEFT, RIGHT
width = RIGHT - LEFT
if width == -1-w: # a test if red blob was found
return None # else:
height = BOTTOM - TOP
xpos = LEFT + width/2
ypos = TOP + height/2
return (xpos, ypos),(width, height)
theta = dat.get('theta', 0)
psi = dat.get('psi', 0)
vx = dat.get('vx', 0)
vy = dat.get('vy', 0)
vz = dat.get('vz', 0)
altitude = dat.get('altitude', 0)
f.write(str_out.format(phi, theta, psi, vx, vy, vz, altitude))
f.flush()
#ret, img = cam.read()
img = video_sensor.get_data()
img = cv2.cvtColor( img, cv.CV_RGB2BGR )
#cv2.imshow('cam', img)
cv.WriteFrame(wr, utils.array2cv(img))
#wr.write(img)
ch = cv.WaitKey(10)
if 0 < ch:
break
#wr.release()
# video_sensor.stop()
# navdata_sensor.stop()
#ci.stop()
f.close()
# jc.stop()
# ci.stop()
# f.close()
drone.stop()
