def getNearestNextNode(self) :
nearDist = 1000000
# if next node is already the destination
if ((self.pathListIndex + 2) >= len(self.pathList)) :
return self.pathListIndex
for i in xrange(self.pathListIndex+2, len(self.pathList)) :
nodeX = int(self.comMap[self.mapNumber].getLocationXCoord(self.pathList[i]))
nodeY = int(self.comMap[self.mapNumber].getLocationYCoord(self.pathList[i]))
distTo = distAngleCalc.distance(self.curX, self.curY, nodeX, nodeY)
if distTo < nearDist :
nearestNodeIndex = i
nearDist = distTo
return nearestNodeIndex
def navigate(self) :
distanceToNode = distAngleCalc.distance(self.curXCoord, self.curYCoord, self.nexXCoord, self.nexYCoord)
self.alertNearingNode(distanceToNode)
if (distanceToNode > self.maxTolerance) :
print "distance to node is: " + str(distanceToNode)
turnAngle = self.getTurnAngle()
# if angle is within tolerance, continue in current direction
# or if not allowed to turn because already turned 90degrees
if (math.fabs(turnAngle) < self.angleTolerance):
sentence = "Go"
print sentence
if time.time() - self.prev_message_time_turn > self.message_delay:
if self.voiceQueue.append(sentence, time.time()):
self.voiceSema.release()
self.prev_message_time_turn = time.time()
return 0
if (turnAngle > 0) :
sentence = "Right " + self.stringNumbers(turnAngle)
print sentence
if time.time() - self.prev_message_time_turn > self.message_delay:
if self.voiceQueue.append(sentence, time.time()):
self.voiceSema.release()
self.prev_message_time_turn = time.time()
elif (turnAngle < 0) :
sentence = "Left " + self.stringNumbers(math.fabs(turnAngle))
print sentence
if time.time() - self.prev_message_time_turn > self.message_delay:
if self.voiceQueue.append(sentence, time.time()):
self.voiceSema.release()
self.prev_message_time_turn = time.time()
else :
sentence = "Go"
print sentence
if time.time() - self.prev_message_time_str > self.message_delay:
if self.voiceQueue.append(sentence, time.time()):
self.voiceSema.release()
self.prev_message_time_turn = time.time()
return 0
else :
self.alreadyAlerted = False
return 1
def fillAMatrix(self) :
# angle always is calculated from the smaller index to the larger index
# negative angle means turn left, positive means right
for i in range(self.numElements) :
self.linkString = self.buildingMap['map'][i]['linkTo']
self.linkArray = [int(s) for s in self.linkString.split(',')]
x1 = int(self.buildingMap['map'][i]['x'])
y1 = int(self.buildingMap['map'][i]['y'])
for j in self.linkArray :
x2 = int(self.buildingMap['map'][j-1]['x'])
y2 = int(self.buildingMap['map'][j-1]['y'])
if self.matrix[j-1][i] == 0 :
self.matrix[i][j-1] = distAngleCalc.distance(
x1, y1, x2, y2)
else :
self.matrix[i][j-1] = distAngleCalc.calcAngle(
x2, y2, x1, y1, self.northAt)
def ignoreNodeObstacle(self) :
distTo = distAngleCalc.distance(self.curX, self.curY, self.nexX, self.nexY)
if distTo < self.maxTolerance :
sentence = "Node Reached."
print sentence
if self.voiceQueue.append_high(sentence, time.time()):
self.voiceSema.release()
self.pathListIndex += 1
self.alertNodeReached()
if self.pathListIndex < (len(self.pathList) - 1) :
self.updatePrevNexCoord()
self.provideNexNodeDirections()
else :
sentence = "Navigation complete."
print sentence
if self.voiceQueue.append_high(sentence, time.time()):
self.voiceSema.release()
self.prev_message_time_turn = time.time()
return True
