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