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()