import cv2;
import Locator;
import time;
cap = cv2.VideoCapture(0)
while(True):
ret, frame = cap.read()
print "here"
pos=Locator.getRobotPos(frame)
cv2.imshow("blah",frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
"""
for i in range(30):
ret, frame = cap.read()
ret, frame = cap.read()
cv2.imwrite('robotPic.png',frame)
cap.release()
cv2.destroyAllWindows()
"""
Mover.spin() #Spins in circle to gather data for gmapping
time.sleep(3)
(trans,rot)=posListener.getPos()
print "(trans,rot) "+str((trans,rot))
(gmap,gpos, gres)=mapClient.getMapAndPos(trans,rot)
#Now marking target as driveable
for i in range(targetPos[0]-targetPos[2],targetPos[0]+targetPos[2]):
if i<0 or i>gmap.shape[0]:
continue
for j in range(targetPos[0]-targetPos[2],targetPos[0]+targetPos[2]):
if i<0 or i>gmap.shape[1]:
continue
gmap[i,j]=0
junk,overheadPic=cap.read()
overheadPos=Locator.getRobotPos(overheadPic)
if onTarget(overheadPos,targetPos):
print "On target"
os.system('say "on target"')
break
scale=overheadPos[3]
mapClient.showMapAndPos(trans,rot)
cv2.circle(overheadPic,(targetPos[0],targetPos[1]),10,(0,0,255))
cv2.imshow("Overhead",overheadPic)
cv2.waitKey(0)
o=newOverlay.Overlay(gmap,overheadPic,gpos,overheadPos,gres)
one2one=o.overlay()
one2onepic=np.zeros((np.shape(one2one)[0],np.shape(one2one)[1],3))
one2onepic[one2one==-1]=(0,0,0)#unknown
