def run(self):
waypoints=None
dist=-1
while True:
preds=self.learnAndPredict()
cv2.waitKey(0)
(dist,waypoints)=Router.getRouteWP(preds.tolist(),(dist,waypoints),(self.sensors.getPos()[1],self.sensors.getPos()[0]),(self.sensors.getTargetPos()[1],self.sensors.getTargetPos()[0]),self.radius,1.5)
self.sensors.showBot(waypoints)
cv2.waitKey(0)
print "Cur Pos"+str(self.sensors.getPos())
print "Waypoints: "+str(waypoints)
distMoved=0
prev=(self.sensors.getPos()[1],self.sensors.getPos()[0])
count=0
for wp in waypoints:
count+=1
distMoved+=Sensors.SimSensors.euclid(prev,wp)
prev=wp
self.sensors.move([wp[1],wp[0]])
if distMoved>Robot.moveDistance:
break
self.sensors.showBot(waypoints[count:])
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
one2onepic[one2one==0]=(0,255,0)#Driveable
one2onepic[one2one==1]=(255,0,0)#wall
cv2.imshow("Known 3d Data overlayed on overhead",one2onepic)
cv2.waitKey(0)
model=LRModel.LRModel(overheadPic,one2one)
preds=model.predictAndShow(selfPos=overheadPos)
print np.shape(one2one)
print np.shape(preds)
retVal=Router.getRouteWP(preds.tolist(),(-1,None),(overheadPos[1],overheadPos[0]),(targetPos[1],targetPos[0]),radius,.5,one2one.tolist())
if retVal==None:
print "No path from here to target"
os.system('say "No path"')
(dist,waypoints)=(prevDist, prevWaypoints)
else:
(dist,waypoints)=retVal
print waypoints
showData(overheadPic,overheadPos,targetPos,waypoints)
startTime=time.time()
index=0
#Actually moving along the waypoints for a set amount of time
for waypoint in waypoints[0]:
if startTime+timeBetweenRelearn<time.time():#Exceeded allotted time
break
junk,overheadPic=cap.read()
