def moveFromCurrentPositionTo(toLocation): global currentPosition angleToTurn = utility.getAngle(currentPosition, toLocation, currentAngle) distanceToMove = utility.getDistance(currentPosition, toLocation, cmPerBox) turn(angleToTurn) while arduino.getRobotMotorMovingStatus(): pass move(distanceToMove) while arduino.getRobotMotorMovingStatus(): pass print WHITE+"turn: "+str(angleToTurn)+" distance: "+str(distanceToMove) currentPosition=toLocation
def orderLineSiteSeq(busline_id): endSites = icfg.db.query("""SELECT * FROM LineSites,Sites WHERE Sites.id=LineSites.site_id AND LineSites.busline_id = {0} AND LineSites.is_end ="yes" ORDER BY LineSites.seq """.format(busline_id)) if len(endSites) < 2: return fromSite = endSites[0] toSite = endSites[1] _Sites = icfg.db.query("""SELECT * FROM LineSites,Sites WHERE Sites.id=LineSites.site_id AND LineSites.busline_id = {0} AND (LineSites.is_end <> "yes" OR LineSites.is_end IS NULL) ORDER BY LineSites.seq """.format(busline_id)) lenSites = len(_Sites) if fromSite.seq != 1: site_id = fromSite.site_id icfg.db.update("LineSites", where="busline_id=$busline_id AND site_id=$site_id", vars=locals(), seq=1) if toSite.seq != 2 + lenSites: site_id = toSite.site_id icfg.db.update("LineSites", where="busline_id=$busline_id AND site_id=$site_id", vars=locals(), seq=2 + lenSites) #保存站点信息,#距离排序 distances = {} sites = {} for i in range(lenSites): sites[i] = _Sites[i] distances[i] = uTools.getDistance(fromSite.gpsLat, fromSite.gpsLng, sites[i].gpsLat, sites[i].gpsLng) #站点排序 dist = sorted(distances.iteritems(), key=lambda d: d[1]) for i in range(lenSites): site_id = sites[dist[i][0]].site_id icfg.db.update("LineSites", where="busline_id=$busline_id AND site_id=$site_id", vars=locals(), seq=1 + i) return
def addIdleEdges(self): ''' Adding idle edges to make the graph fully complete :return: ''' nodes = self.graph.node for start_node, start_info in nodes.iteritems(): for end_node, end_info in nodes.iteritems(): if start_node != end_node and not self.graph.has_edge( start_node, end_node): distance = utility.getDistance(self.graph, start_node, end_node) self.graph.add_edge(start_node, end_node, IDLE_COST=distance)
def addIdleEdges(self): ''' Adding Idle edges to the graph ''' for start_node, edges in self.graph.edge.iteritems(): other_nodes = Counter(self.getOtherNodes(start_node)) adjacent_nodes = [] for end_node, weight in edges.iteritems(): adjacent_nodes.append(end_node) adjacent_nodes = Counter(adjacent_nodes) missing_nodes = other_nodes - adjacent_nodes for node in missing_nodes: weight = utility.getDistance(self.graph, start_node, end_node) self.graph.add_edge(start_node, node, weight=weight, IDLE_EDGES=True) pass
def getSegmentLength(self): dist = 0 for index,item in enumerate(self.segCoordinates): if not(index == len(self.segCoordinates)-1): dist += utility.getDistance(self.segCoordinates[index][0],self.segCoordinates[index][1],self.segCoordinates[index+1][0],self.segCoordinates[index+1][1]) return dist
def phraseOne(): setRunningPhrase(1) threading.Thread(target=utility.startTimer).start() path = [] goBack=False getPath=True #getPath = True #turn 90 at very first while True: #to return on the same path used isPath=True time.sleep(1) if(endPosition==currentPosition): sendMessages("Reached EndPosition in "+str(utility.getTime())+" seconds",3) #print "=========================>>>>> reach endpoint" #phraseOneCheckIfAtGoal() break try: if(currentPosition==path[0]): path.pop(0) except: pass #print "dddd" #if getPath is true, get optimal path (90degrees) if(getPath or len(path)==0): getPath = False sendMessages("currentAngle "+str(currentAngle),3) sendMessages("currentPosition "+str(currentPosition[0])+" "+str(currentPosition[1]),3) sendMessages("phraseOneFindOptimalPath",1) returnMsgInString = comms.getMsg() #print "aaaaaa" #print returnMsgInString returnMsgInString = returnMsgInString.split(';') pathInString = returnMsgInString[0] pathInString = pathInString.translate(string.maketrans('', ''), '[] ') pathInString = pathInString.split(',') changeSafetyDistance = utility.parseBoolString(returnMsgInString[1]); #print "bbbb" #print changeSafetyDistance #print pathInString ''' if (pathInString=='[]'): goBack=True continue ''' path=[] i=0 #print "======== "+str(pathInString) while i<len(pathInString): path.append([int(pathInString[i]),int(pathInString[i+1])]) i=i+2 print "get new path:::" print path #base on path, get direction to face directionToFace = utility.getAngle(currentPosition, path[0], currentAngle) distanceToNextPoint = utility.getDistance(currentPosition, path[0], cmPerBox) #print ("direction "+str(directionToFace)) #print ("distance "+str(distanceToNextPoint)) print "currentPosition = "+str(currentPosition) if(directionToFace!=0): if(currentPosition==startPosition): isPath=False elif(len(returnPath)>0): if(returnPath[0]==currentPosition): isPath=False if(isPath): newPoint=[0,0] newPoint[0]=currentPosition[0] newPoint[1]=currentPosition[1] returnPath.insert(0,newPoint) turn(directionToFace) while arduino.getRobotMotorMovingStatus(): pass #print (WHITE+"current position "+str(currentPosition)) #print (WHITE+"current angle "+str(currentAngle)) #print (WHITE+"disatnce to next point"+str(distanceToNextPoint)) #check left and right sensor for obs sensorValues = arduino.getSensorStatus() #print WHITE+"sensor values" #print WHITE+sensorValues sensorValues = sensorValues.split() isLeftObstacle=utility.parseBoolString(sensorValues[4]) isRightObstacle=utility.parseBoolString(sensorValues[6]) print "sensor values:::::" print sensorValues if (currentPosition[0]>=34 and currentPosition[1]>=24 and (isRightObstacle or isLeftObstacle)): sendMessages("Reached EndPosition in "+str(utility.getTime())+" seconds",3) print "not very accurate, but regard as reached" #print "=========================>>>>> reach endpoint" #phraseOneCheckIfAtGoal() break if(isRightObstacle):#if right sensor is true #print ("current "+str(currentPosition)) #print "currentPosition = "+str(currentPosition) #print "left obstacle" setObstacle("right",10) obsTrueCoords = comms.getMsg() sendMessages("map setObstacle "+obsTrueCoords,2) getPath=True #bottom sensor is for front left if(isLeftObstacle):#if bottom sensor is true, front obs within 10cm #print ("current "+str(currentPosition)) #print "currentPosition = "+str(currentPosition) #print "right obstacle" setObstacle("left",10) obsTrueCoords = comms.getMsg() sendMessages("map setObstacle "+obsTrueCoords,2) getPath=True if ((not getPath) or changeSafetyDistance): move(10) changeSafetyDistance=False else: continue while arduino.getRobotMotorMovingStatus(): pass continue endingCheck()
path=[] i=0 print "======== "+str(pathInString) while i<len(pathInString): path.append([int(pathInString[i]),int(pathInString[i+1])]) i=i+2 print "get new path:::" print path #base on path, get direction to face directionToFace = utility.getAngle(currentPosition, path[0], currentAngle) distanceToNextPoint = utility.getDistance(currentPosition, path[0], cmPerBox) #print ("direction "+str(directionToFace)) #print ("distance "+str(distanceToNextPoint)) print "currentPosition = "+str(currentPosition) if(directionToFace!=0): if(currentPosition==startPosition): isPath=False elif(len(returnPath)>0): if(returnPath[0]==currentPosition): isPath=False if(isPath): newPoint=[0,0] newPoint[0]=currentPosition[0] newPoint[1]=currentPosition[1] returnPath.insert(0,newPoint) turn(directionToFace)
def phraseOne(): setRunningPhrase(1) threading.Thread(target=utility.startTimer).start() path = [] goBack = False getPath = True #getPath = True #turn 90 at very first while True: #to return on the same path used isPath = True time.sleep(1) if (endPosition == currentPosition): sendMessages( "Reached EndPosition in " + str(utility.getTime()) + " seconds", 3) #print "=========================>>>>> reach endpoint" #phraseOneCheckIfAtGoal() break try: if (currentPosition == path[0]): path.pop(0) except: pass #print "dddd" #if getPath is true, get optimal path (90degrees) if (getPath or len(path) == 0): getPath = False sendMessages("currentAngle " + str(currentAngle), 3) sendMessages( "currentPosition " + str(currentPosition[0]) + " " + str(currentPosition[1]), 3) sendMessages("phraseOneFindOptimalPath", 1) returnMsgInString = comms.getMsg() #print "aaaaaa" #print returnMsgInString returnMsgInString = returnMsgInString.split(';') pathInString = returnMsgInString[0] pathInString = pathInString.translate(string.maketrans('', ''), '[] ') pathInString = pathInString.split(',') changeSafetyDistance = utility.parseBoolString( returnMsgInString[1]) #print "bbbb" #print changeSafetyDistance #print pathInString ''' if (pathInString=='[]'): goBack=True continue ''' path = [] i = 0 #print "======== "+str(pathInString) while i < len(pathInString): path.append([int(pathInString[i]), int(pathInString[i + 1])]) i = i + 2 print "get new path:::" print path #base on path, get direction to face directionToFace = utility.getAngle(currentPosition, path[0], currentAngle) distanceToNextPoint = utility.getDistance(currentPosition, path[0], cmPerBox) #print ("direction "+str(directionToFace)) #print ("distance "+str(distanceToNextPoint)) print "currentPosition = " + str(currentPosition) if (directionToFace != 0): if (currentPosition == startPosition): isPath = False elif (len(returnPath) > 0): if (returnPath[0] == currentPosition): isPath = False if (isPath): newPoint = [0, 0] newPoint[0] = currentPosition[0] newPoint[1] = currentPosition[1] returnPath.insert(0, newPoint) turn(directionToFace) while arduino.getRobotMotorMovingStatus(): pass #print (WHITE+"current position "+str(currentPosition)) #print (WHITE+"current angle "+str(currentAngle)) #print (WHITE+"disatnce to next point"+str(distanceToNextPoint)) #check left and right sensor for obs sensorValues = arduino.getSensorStatus() #print WHITE+"sensor values" #print WHITE+sensorValues sensorValues = sensorValues.split() isLeftObstacle = utility.parseBoolString(sensorValues[4]) isRightObstacle = utility.parseBoolString(sensorValues[6]) print "sensor values:::::" print sensorValues if (currentPosition[0] >= 34 and currentPosition[1] >= 24 and (isRightObstacle or isLeftObstacle)): sendMessages( "Reached EndPosition in " + str(utility.getTime()) + " seconds", 3) print "not very accurate, but regard as reached" #print "=========================>>>>> reach endpoint" #phraseOneCheckIfAtGoal() break if (isRightObstacle): #if right sensor is true #print ("current "+str(currentPosition)) #print "currentPosition = "+str(currentPosition) #print "left obstacle" setObstacle("right", 10) obsTrueCoords = comms.getMsg() sendMessages("map setObstacle " + obsTrueCoords, 2) getPath = True #bottom sensor is for front left if (isLeftObstacle): #if bottom sensor is true, front obs within 10cm #print ("current "+str(currentPosition)) #print "currentPosition = "+str(currentPosition) #print "right obstacle" setObstacle("left", 10) obsTrueCoords = comms.getMsg() sendMessages("map setObstacle " + obsTrueCoords, 2) getPath = True if ((not getPath) or changeSafetyDistance): move(10) changeSafetyDistance = False else: continue while arduino.getRobotMotorMovingStatus(): pass continue endingCheck()