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