def get_nav(self):
if self.nav:
return self.nav
if(self.is_same_map()): # normal same-map navigation
startName = Map.get_node_by_location_id(self.startBuilding, self.startLevel, self.startId)['nodeName']
endName = Map.get_node_by_location_id(self.endBuilding, self.endLevel, self.endId)['nodeName']
self.nav.append(Navigation(self.startBuilding, self.startLevel, startName, endName))
else: # inter-building or inter-level navigation
mapRoute = self.get_map_route()
mapRouteIdx = 0
for map in mapRoute:
building, level = map.split("-")
startName = ""
endName = ""
if mapRouteIdx == 0: # start map
startName = Map.get_node_by_location_id(self.startBuilding, self.startLevel, self.startId)['nodeName'] # start is given
endName = Map.get_node_by_connected_map(self.startBuilding, self.startLevel, mapRoute[mapRouteIdx + 1])['nodeName']
elif mapRouteIdx == (len(mapRoute) - 1): # end map
# print "the end"
prevNav = self.nav[mapRouteIdx - 1]
# print "prev nav end is " + prevNav.end
startId = self.extract_id_from_name(prevNav.end)
# print "id is " + startId
startName = Map.get_node_by_location_id(self.endBuilding, self.endLevel, startId)['nodeName']
# print "start name is " + startName
endName = Map.get_node_by_location_id(self.endBuilding, self.endLevel, self.endId)['nodeName'] # end is given
else: # in the mid
# print "in the mid"
prevNav = self.nav[mapRouteIdx - 1]
# print "prev nav end is " + prevNav.end
startId = self.extract_id_from_name(prevNav.end)
# print "id is " + startId
startName = Map.get_node_by_location_id(building, level, startId)['nodeName']
# print "start name is " + startName
endName = Map.get_node_by_connected_map(building, level, mapRoute[mapRouteIdx + 1])['nodeName']
self.nav.append(Navigation(building, level, startName, endName))
mapRouteIdx += 1
return self.nav
def get_map_route(self):
visited = set()
visited.add(self.startBuilding + "-" + self.startLevel)
queue = [self.startBuilding + "-" + self.startLevel]
parent = {}
while queue:
u = queue.pop(0)
building, level = u.split("-")
connectedMaps = Map.get_connected_maps(building, level)
for map in connectedMaps:
nextBuilding, nextLevel, nextId = map.split("-")
if (nextBuilding + "-" + nextLevel) not in visited:
visited.add(nextBuilding + "-" + nextLevel)
queue.append(nextBuilding + "-" + nextLevel)
parent[nextBuilding + "-" + nextLevel] = building + "-" + level
route = [self.endBuilding + "-" + self.endLevel]
tmp = self.endBuilding + "-" + self.endLevel
while tmp != (self.startBuilding + "-" + self.startLevel):
route.append(parent[tmp])
tmp = parent[tmp]
route.reverse()
return route
def run():
voice_output.speak('welcome to navicane system')
(starting_building, starting_level, starting_point,
ending_building, ending_level, ending_point) = get_user_input()
# TODO: may get NONE if input param is incorrect, if incorrect should re-input
# startPtName = Map.get_node_by_location_id(starting_building, starting_level, starting_point)['nodeName']
# endPtName = Map.get_node_by_location_id(ending_building, ending_level, ending_point)['nodeName']
nav = Guidance(starting_building, starting_level, starting_point, ending_building, ending_level, ending_point)
voice_output.speak('start navigation')
voice_output.speak('set up u art connection')
while not check_connection_status():
initiate_connection()
voice_output.speak('set up ok')
is_data_corrupted, sensors_data = receive_data()
if not is_data_corrupted:
give_current_instruction(nav.get_next_instruction(remap_direction(sensors_data[4]))) # next loc's ID
give_current_instruction(nav.get_next_instruction(remap_direction(sensors_data[4]))) # next loc's direction
global totalSteps
totalSteps = int(sensors_data[6])
faster_loop_time = now()
runner = 0
global is_running_mode
while is_running_mode:
isJustCalibrated = False
if not ir_reading_queue.empty():
state = ir_reading_queue.get(block=False)
else:
state = 0
while not check_connection_status():
initiate_connection()
if state == 1: # special pattern recognized! the actual pos for the special node
pos = nav.get_pos()
print '\npattern is 1\n'
print nav.get_next_loc()
print nav.get_prev_loc()
if SpecialNode.is_special_node(nav.get_curr_building(), nav.get_curr_level(), nav.get_next_loc()):
# reach the actual important loc but not reach the point on the map
if Map.get_distance(pos[0], nav.get_next_loc()['x'], pos[1], nav.get_next_loc()['y']) < 500 and \
nav.get_next_loc()['nodeName'] not in calibratedNodes:
calibratedNodes.append(nav.get_next_loc()['nodeName'])
nav.reach_special_node(nav.get_next_loc())
isJustCalibrated = True
elif SpecialNode.is_special_node(nav.get_curr_building(), nav.get_curr_level(), nav.get_prev_loc()):
print 'prev is special'
# reach the actual important loc but map shows passed the node already
if Map.get_distance(pos[0], nav.get_prev_loc()['x'], pos[1], nav.get_prev_loc()['y']) < 500 and \
nav.get_prev_loc()['nodeName'] not in calibratedNodes:
print nav.get_prev_loc()['nodeName']
calibratedNodes.append(nav.get_prev_loc()['nodeName'])
nav.reach_special_node(nav.get_prev_loc())
isJustCalibrated = True
if isJustCalibrated:
voice_output.speak('calibrated')
if now() - faster_loop_time > FASTER_LOOP_TIMER:
is_data_corrupted, sensors_data = receive_data()
if not is_data_corrupted:
logger.info('front right: {0}; front left: {1};right: {2};left: {3}'.format(str(sensors_data[0]),
str(sensors_data[1]),
str(sensors_data[2]),
str(sensors_data[3])))
logger.info('compass: {0}; barometer: {1};distance: {2}'.format(str(sensors_data[4]),
str(sensors_data[5]),
str(sensors_data[6])))
ultrasonic_handle.feed_data(sensors_data[1], sensors_data[0],
sensors_data[3], sensors_data[2])
deltaDist = remap_distance(sensors_data[6])
print "delta dist is " + str(deltaDist)
if not isJustCalibrated:
nav.update_pos_by_dist_and_dir(deltaDist, remap_direction(sensors_data[4]))
print "current pos is" # TODO: remove this after eval 2 drill
print nav.get_pos() # TODO: remove this after eval 2 drill
print "next location pos is" # TODO: remove this after eval 2 drill
print "[" + nav.get_next_loc()["x"] + ", " + nav.get_next_loc()["y"] + "]"
if runner == 0:
if nav.is_reach_next_location():
voice_output.speak('you just reached {0}'.format(str(nav.get_next_loc()["nodeId"])))
if not nav.is_reach_end():
give_current_instruction(nav.get_next_instruction(remap_direction(sensors_data[4])))
else:
give_current_instruction(REACH_END)
is_running_mode = False
else:
give_current_instruction(nav.get_next_instruction(remap_direction(sensors_data[4])))
else:
if nav.is_reach_next_location():
voice_output.speak('you just reached {0}'.format(str(nav.get_next_loc()["nodeId"])))
if not nav.is_reach_end():
give_current_instruction(nav.get_next_instruction(remap_direction(sensors_data[4])))
else:
give_current_instruction(REACH_END)
is_running_mode = False
else:
give_current_instruction()
runner = (runner + 1) % 2
faster_loop_time = now()
