def __init__(self, logger):
self.log = logger
self.log.info('Starting navigator...')
self.db = DB(logger=logger)
self.maps = MapsRepo()
self.audio = AudioDriver()
self.sc = StepCounter(logger)
self.hc = HeadingCalculator(logger)
self.cam = Process(target=camera, name="Camera", args=(QUEUE,))
self.current_prompt = None
self.navi_chunk_finished = False
self.heading_timestamp = utils.now()
GPIO.setwarnings(False)
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
GPIO.setup(GPIO_OVERRIDE_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
def __init__(self, logger):
self.log = logger
self.log.info("Starting navigator...")
self.db = DB(logger=logger)
self.maps = MapsRepo()
self.audio = AudioDriver()
self.sc = StepCounter(logger)
self.hc = HeadingCalculator(logger)
self.cam = Process(target=camera, name="Camera", args=(QUEUE,))
self.current_prompt = None
self.navi_chunk_finished = False
self.heading_timestamp = utils.now()
GPIO.setwarnings(False)
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
GPIO.setup(GPIO_OVERRIDE_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
class Navigator(object):
def __init__(self, logger):
self.log = logger
self.log.info("Starting navigator...")
self.db = DB(logger=logger)
self.maps = MapsRepo()
self.audio = AudioDriver()
self.sc = StepCounter(logger)
self.hc = HeadingCalculator(logger)
self.cam = Process(target=camera, name="Camera", args=(QUEUE,))
self.current_prompt = None
self.navi_chunk_finished = False
self.heading_timestamp = utils.now()
GPIO.setwarnings(False)
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
GPIO.setup(GPIO_OVERRIDE_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
@property
def next_node(self):
return self.graph[self.next_node_id]
@property
def next_node_id(self):
return self.path[self.next_node_idx]
@property
def current_node(self):
return self.graph[self.current_node_id]
@property
def current_node_id(self):
return self.path[self.next_node_idx - 1]
@property
def prev_node(self):
prev_node_id = self.prev_node_id
if prev_node_id:
return self.graph[prev_node_id]
else:
return {"x": None, "y": None}
@property
def prev_node_id(self):
if self.next_node_idx >= 2:
return self.path[self.next_node_idx - 2]
else:
return None
def _get_user_heading(self, timestamp=None):
if timestamp is None:
timestamp = self.heading_timestamp
self.hc.clear_filter()
self.hc.set_map_north(self.north)
data = self.db.fetch_data(sid=BACK_SENSOR_ID, since=timestamp)
self.heading_timestamp = data[-1][0]
return self.hc.get_heading([d[2] for d in data])
def _wait_for_origin_and_destination(self):
self.log.info("Waiting for origin and destination...")
data = self.db.fetch_origin_and_destination()
self.o_bldg, self.o_level, self.o_node = data[0], data[1], data[2]
self.d_bldg, self.d_level, self.d_node = data[3], data[4], data[5]
self.log.info(
"Got %s-%s-%s to %s-%s-%s", self.o_bldg, self.o_level, self.o_node, self.d_bldg, self.d_level, self.d_node
)
def _generate_chunks(self):
if self.o_bldg != self.d_bldg or self.o_level != self.d_level:
self.log.info("Creating multiple navi chunks")
maps_to_visit = self._find_maps_to_visit()
self.navi_chunks = self._generate_multiple_chunks(maps_to_visit)
else:
self.log.info("Creating single navi chunk")
self.navi_chunks = [((self.o_bldg, self.o_level, self.o_node), (self.d_bldg, self.d_level, self.d_node))]
self.log.info("Generated chunk(s): " + str(self.navi_chunks))
def _find_maps_to_visit(self):
dist = {(self.o_bldg, self.o_level): 0}
parent = {}
to_visit = deque()
to_visit.append((self.o_bldg, self.o_level))
while len(to_visit) != 0:
bldg, level = to_visit.popleft()
if (bldg, level) == (self.d_bldg, self.d_level):
break
connections = self.maps.connectors(bldg, level)
for b in connections:
for l in connections[b]:
if (b, l) not in dist:
dist[(b, l)] = dist[(bldg, level)] + 1
parent[(b, l)] = (bldg, level)
to_visit.append((b, l))
if (self.d_bldg, self.d_level) not in parent:
raise RuntimeError("No path from origin to destination! :O")
curr = (self.d_bldg, self.d_level)
maps_to_visit = [curr]
while curr != (self.o_bldg, self.o_level):
curr = parent[curr]
maps_to_visit.append(curr)
maps_to_visit = maps_to_visit[::-1]
self.log.info("Will visit maps: " + str(maps_to_visit))
return maps_to_visit
def _generate_multiple_chunks(self, maps_to_visit):
navi_chunks = []
navi_chunk = [(self.o_bldg, self.o_level, self.o_node)]
for i in range(len(maps_to_visit[:-1])):
curr_bldg, curr_lvl = maps_to_visit[i]
n_bldg, n_lvl = maps_to_visit[i + 1]
conns = self.maps.connectors(curr_bldg, curr_lvl)
n_conns = self.maps.connectors(n_bldg, n_lvl)
navi_chunk.append((curr_bldg, curr_lvl, conns[n_bldg][n_lvl]))
navi_chunks.append(navi_chunk)
navi_chunk = [(n_bldg, n_lvl, n_conns[curr_bldg][curr_lvl])]
dst_map_conns = self.maps.connectors(self.d_bldg, self.d_level)
last_dst = navi_chunks[-1][-1]
dst_map_start_node = dst_map_conns[last_dst[0]][last_dst[1]]
navi_chunks.append([(self.d_bldg, self.d_level, dst_map_start_node), (self.d_bldg, self.d_level, self.d_node)])
return navi_chunks
def _prepare_for_next_navi_chunk(self):
current_chunk = self.navi_chunks[0]
self.building = current_chunk[0][0]
self.level = current_chunk[0][1]
self.origin = current_chunk[0][2]
self.destination = current_chunk[1][2]
self.navi_chunk_finished = False
self.log.info("Switching to %s-%s", self.building, self.level)
self.log.info("Endpoints %s-%s", self.origin, self.destination)
def _get_map(self):
self.graph = self.maps.map(self.building, self.level)
self.north = self.maps.north_heading(self.building, self.level)
self.db.insert_location(
self.graph[self.origin]["x"], self.graph[self.origin]["y"], self.north, 0, is_reset=True
)
def _generate_path(self):
self.log.info("Generating path...")
self.path = utils.dijkstra(self.graph, self.origin, self.destination)
self.next_node_idx = 1
self.log.info("Got path: %s", str(self.path))
def _wait_for_chunk_to_finish(self):
while not self.navi_chunk_finished:
self._navigate_to_next_node()
def _navigate_to_next_node(self):
self._align_to_next_node()
self._wait_for_steps_to_next_node()
self._node_reached()
def _align_to_next_node(self):
self.log.info("Aligning...")
angle_to_turn = self._calc_angle_to_turn(
self.prev_node["x"],
self.prev_node["y"],
self.current_node["x"],
self.current_node["y"],
self.next_node["x"],
self.next_node["y"],
)
if angle_to_turn == 0:
self.log.info("No alignment needed")
else:
if angle_to_turn > 0:
self.log.info("Left turn by %d", angle_to_turn)
self.audio.prompt(PromptDirn.left, angle_to_turn)
else:
self.log.info("Right turn by %d", angle_to_turn)
self.audio.prompt(PromptDirn.right, angle_to_turn)
self._wait_for_angle_turn(angle_to_turn)
def _calc_angle_to_turn(self, x1, y1, x2, y2, x3, y3):
if x1 is None or y1 is None: # No prev node (initial alignment)
v_a = [1, 0]
v_b = [(x3 - x2), (y3 - y2)]
node_heading = utils.angle_bw_vectors(v_a, v_b)
user_heading = self._get_user_heading(timestamp=utils.now())
return utils.normalize_360(node_heading - user_heading)
else:
v_a = [(x2 - x1), (y2 - y1)]
v_b = [(x3 - x2), (y3 - y2)]
return utils.angle_bw_vectors(v_a, v_b)
def _wait_for_angle_turn(self, angle_to_turn):
self.log.info("Waiting for turn...")
initial_heading = self._get_user_heading(timestamp=utils.now())
self.log.info("Initial heading: %d", initial_heading)
turned_angle = 0
while abs(turned_angle - angle_to_turn) > ANGLE_THRESHOLD:
user_heading = self._get_user_heading()
if angle_to_turn > 0 and user_heading < initial_heading:
user_heading += 360
elif angle_to_turn < 0 and user_heading > initial_heading:
user_heading -= 360
turned_angle = user_heading - initial_heading
self.log.info("Angle turned: %d (= %d - %d)", turned_angle, user_heading, initial_heading)
self.log.info("Turn complete")
def _wait_for_steps_to_next_node(self):
num_of_steps_to_next_node = self._calc_num_steps_to_next_node()
self.audio.prompt(PromptDirn.straight, num_of_steps_to_next_node)
self._wait_for_steps(num_of_steps_to_next_node)
def _calc_num_steps_to_next_node(self):
dist = utils.euclidean_dist(
self.current_node["x"], self.current_node["y"], self.next_node["x"], self.next_node["y"]
)
num_of_steps_to_next_node = int(round(dist / STEP_LENGTH))
self.log.info("%d steps to next node", num_of_steps_to_next_node)
return num_of_steps_to_next_node
def _wait_for_steps(self, num_of_steps_to_wait):
self.log.info("Waiting for steps...")
self._clear_camera_queue()
timestamp = utils.now()
while num_of_steps_to_wait > 0:
data = self.db.fetch_data(sid=FOOT_SENSOR_ID, since=timestamp)
timestamp = data[-1][0]
for data_pt in [x[2] for x in data]:
if self.sc.detect_step(data_pt) and GPIO.input(GPIO_OVERRIDE_PIN):
num_of_steps_to_wait -= 2
self.log.info("Step to go: %d", num_of_steps_to_wait)
if num_of_steps_to_wait < 6:
self.audio.prompt_step(num_of_steps_to_wait)
else:
self.audio.prompt_step()
if num_of_steps_to_wait <= 0:
break
try:
node_id = QUEUE.get_nowait()
if str(node_id) in self.path:
self.log.info("Captured node %s", node_id)
self.log.info("Resetting steps to 2")
num_of_steps_to_wait = 2
self.next_node_idx = self.path.index(str(node_id))
self.audio.prompt_camera()
except Empty:
continue
self.log.info("Completed steps")
def _clear_camera_queue(self):
while not QUEUE.empty():
try:
QUEUE.get_nowait()
except Empty:
continue
def _node_reached(self):
self.current_prompt = None
self.audio.prompt_node_reached(self.next_node_id)
self.log.info("Reached node %s", self.next_node_id)
if self.next_node["name"] == "Stairwell":
self.audio.prompt_stairs()
elif self.next_node["name"].lower().find("door") != -1:
self.audio.prompt_door()
self.next_node_idx += 1
if self.next_node_idx == len(self.path):
self.navi_chunk_finished = True
self.log.info("Reached destination node")
else:
if self.next_node["name"].lower().find("office") != -1 or self.next_node["name"].lower().find("room") != -1:
self.audio.prompt_door()
self.log.info("Navigating to node #%s %s", self.next_node_id, self.next_node["name"])
def start(self):
self.cam.start()
self._wait_for_origin_and_destination()
self._generate_chunks()
while len(self.navi_chunks) != 0:
self._prepare_for_next_navi_chunk()
self._get_map()
self._generate_path()
self._wait_for_chunk_to_finish()
self.navi_chunks.pop(0)
self.audio.prompt(PromptDirn.end)
class Navigator(object):
def __init__(self, logger):
self.log = logger
self.log.info('Starting navigator...')
self.db = DB(logger=logger)
self.maps = MapsRepo()
self.audio = AudioDriver()
self.sc = StepCounter(logger)
self.hc = HeadingCalculator(logger)
self.cam = Process(target=camera, name="Camera", args=(QUEUE,))
self.current_prompt = None
self.navi_chunk_finished = False
self.heading_timestamp = utils.now()
GPIO.setwarnings(False)
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
GPIO.setup(GPIO_OVERRIDE_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP)
@property
def next_node(self):
return self.graph[self.next_node_id]
@property
def next_node_id(self):
return self.path[self.next_node_idx]
@property
def current_node(self):
return self.graph[self.current_node_id]
@property
def current_node_id(self):
return self.path[self.next_node_idx - 1]
@property
def prev_node(self):
prev_node_id = self.prev_node_id
if prev_node_id:
return self.graph[prev_node_id]
else:
return {'x': None, 'y': None}
@property
def prev_node_id(self):
if self.next_node_idx >= 2:
return self.path[self.next_node_idx - 2]
else:
return None
def _get_user_heading(self, timestamp=None):
if timestamp is None:
timestamp = self.heading_timestamp
self.hc.clear_filter()
self.hc.set_map_north(self.north)
data = self.db.fetch_data(sid=BACK_SENSOR_ID, since=timestamp)
self.heading_timestamp = data[-1][0]
return self.hc.get_heading([d[2] for d in data])
def _wait_for_origin_and_destination(self):
self.log.info('Waiting for origin and destination...')
data = self.db.fetch_origin_and_destination()
self.o_bldg, self.o_level, self.o_node = data[0], data[1], data[2]
self.d_bldg, self.d_level, self.d_node = data[3], data[4], data[5]
self.log.info('Got %s-%s-%s to %s-%s-%s',
self.o_bldg, self.o_level, self.o_node,
self.d_bldg, self.d_level, self.d_node)
def _generate_chunks(self):
if self.o_bldg != self.d_bldg or self.o_level != self.d_level:
self.log.info('Creating multiple navi chunks')
maps_to_visit = self._find_maps_to_visit()
self.navi_chunks = self._generate_multiple_chunks(maps_to_visit)
else:
self.log.info('Creating single navi chunk')
self.navi_chunks = [((self.o_bldg, self.o_level, self.o_node),
(self.d_bldg, self.d_level, self.d_node))]
self.log.info("Generated chunk(s): " + str(self.navi_chunks))
def _find_maps_to_visit(self):
dist = {(self.o_bldg, self.o_level): 0}
parent = {}
to_visit = deque()
to_visit.append((self.o_bldg, self.o_level))
while len(to_visit) != 0:
bldg, level = to_visit.popleft()
if (bldg, level) == (self.d_bldg, self.d_level):
break
connections = self.maps.connectors(bldg, level)
for b in connections:
for l in connections[b]:
if (b, l) not in dist:
dist[(b, l)] = dist[(bldg, level)] + 1
parent[(b, l)] = (bldg, level)
to_visit.append((b, l))
if (self.d_bldg, self.d_level) not in parent:
raise RuntimeError("No path from origin to destination! :O")
curr = (self.d_bldg, self.d_level)
maps_to_visit = [curr]
while curr != (self.o_bldg, self.o_level):
curr = parent[curr]
maps_to_visit.append(curr)
maps_to_visit = maps_to_visit[::-1]
self.log.info('Will visit maps: ' + str(maps_to_visit))
return maps_to_visit
def _generate_multiple_chunks(self, maps_to_visit):
navi_chunks = []
navi_chunk = [(self.o_bldg, self.o_level, self.o_node)]
for i in range(len(maps_to_visit[:-1])):
curr_bldg, curr_lvl = maps_to_visit[i]
n_bldg, n_lvl = maps_to_visit[i + 1]
conns = self.maps.connectors(curr_bldg, curr_lvl)
n_conns = self.maps.connectors(n_bldg, n_lvl)
navi_chunk.append((curr_bldg, curr_lvl, conns[n_bldg][n_lvl]))
navi_chunks.append(navi_chunk)
navi_chunk = [(n_bldg, n_lvl, n_conns[curr_bldg][curr_lvl])]
dst_map_conns = self.maps.connectors(self.d_bldg, self.d_level)
last_dst = navi_chunks[-1][-1]
dst_map_start_node = dst_map_conns[last_dst[0]][last_dst[1]]
navi_chunks.append([(self.d_bldg, self.d_level, dst_map_start_node),
(self.d_bldg, self.d_level, self.d_node)])
return navi_chunks
def _prepare_for_next_navi_chunk(self):
current_chunk = self.navi_chunks[0]
self.building = current_chunk[0][0]
self.level = current_chunk[0][1]
self.origin = current_chunk[0][2]
self.destination = current_chunk[1][2]
self.navi_chunk_finished = False
self.log.info("Switching to %s-%s", self.building, self.level)
self.log.info("Endpoints %s-%s", self.origin, self.destination)
def _get_map(self):
self.graph = self.maps.map(self.building, self.level)
self.north = self.maps.north_heading(self.building, self.level)
self.db.insert_location(self.graph[self.origin]['x'],
self.graph[self.origin]['y'],
self.north, 0, is_reset=True)
def _generate_path(self):
self.log.info('Generating path...')
self.path = utils.dijkstra(self.graph, self.origin, self.destination)
self.next_node_idx = 1
self.log.info('Got path: %s', str(self.path))
def _wait_for_chunk_to_finish(self):
while not self.navi_chunk_finished:
self._navigate_to_next_node()
def _navigate_to_next_node(self):
self._align_to_next_node()
self._wait_for_steps_to_next_node()
self._node_reached()
def _align_to_next_node(self):
self.log.info("Aligning...")
angle_to_turn = self._calc_angle_to_turn(
self.prev_node['x'], self.prev_node['y'],
self.current_node['x'], self.current_node['y'],
self.next_node['x'], self.next_node['y']
)
if angle_to_turn == 0:
self.log.info("No alignment needed")
else:
if angle_to_turn > 0:
self.log.info("Left turn by %d", angle_to_turn)
self.audio.prompt(PromptDirn.left, angle_to_turn)
else:
self.log.info("Right turn by %d", angle_to_turn)
self.audio.prompt(PromptDirn.right, angle_to_turn)
self._wait_for_angle_turn(angle_to_turn)
def _calc_angle_to_turn(self, x1, y1, x2, y2, x3, y3):
if x1 is None or y1 is None: # No prev node (initial alignment)
v_a = [1, 0]
v_b = [(x3 - x2), (y3 - y2)]
node_heading = utils.angle_bw_vectors(v_a, v_b)
user_heading = self._get_user_heading(timestamp=utils.now())
return utils.normalize_360(node_heading - user_heading)
else:
v_a = [(x2 - x1), (y2 - y1)]
v_b = [(x3 - x2), (y3 - y2)]
return utils.angle_bw_vectors(v_a, v_b)
def _wait_for_angle_turn(self, angle_to_turn):
self.log.info("Waiting for turn...")
initial_heading = self._get_user_heading(timestamp=utils.now())
self.log.info("Initial heading: %d", initial_heading)
turned_angle = 0
while abs(turned_angle - angle_to_turn) > ANGLE_THRESHOLD:
user_heading = self._get_user_heading()
if angle_to_turn > 0 and user_heading < initial_heading:
user_heading += 360
elif angle_to_turn < 0 and user_heading > initial_heading:
user_heading -= 360
turned_angle = user_heading - initial_heading
self.log.info("Angle turned: %d (= %d - %d)", turned_angle,
user_heading, initial_heading)
self.log.info("Turn complete")
def _wait_for_steps_to_next_node(self):
num_of_steps_to_next_node = self._calc_num_steps_to_next_node()
self.audio.prompt(PromptDirn.straight, num_of_steps_to_next_node)
self._wait_for_steps(num_of_steps_to_next_node)
def _calc_num_steps_to_next_node(self):
dist = utils.euclidean_dist(
self.current_node['x'], self.current_node['y'],
self.next_node['x'], self.next_node['y']
)
num_of_steps_to_next_node = int(round(dist / STEP_LENGTH))
self.log.info("%d steps to next node", num_of_steps_to_next_node)
return num_of_steps_to_next_node
def _wait_for_steps(self, num_of_steps_to_wait):
self.log.info("Waiting for steps...")
self._clear_camera_queue()
timestamp = utils.now()
while num_of_steps_to_wait > 0:
data = self.db.fetch_data(sid=FOOT_SENSOR_ID, since=timestamp)
timestamp = data[-1][0]
for data_pt in [x[2] for x in data]:
if self.sc.detect_step(data_pt) and \
GPIO.input(GPIO_OVERRIDE_PIN):
num_of_steps_to_wait -= 2
self.log.info("Step to go: %d", num_of_steps_to_wait)
if num_of_steps_to_wait < 6:
self.audio.prompt_step(num_of_steps_to_wait)
else:
self.audio.prompt_step()
if num_of_steps_to_wait <= 0:
break
try:
node_id = QUEUE.get_nowait()
if str(node_id) in self.path:
self.log.info('Captured node %s', node_id)
self.log.info('Resetting steps to 2')
num_of_steps_to_wait = 2
self.next_node_idx = self.path.index(str(node_id))
self.audio.prompt_camera()
except Empty:
continue
self.log.info("Completed steps")
def _clear_camera_queue(self):
while not QUEUE.empty():
try:
QUEUE.get_nowait()
except Empty:
continue
def _node_reached(self):
self.current_prompt = None
self.audio.prompt_node_reached(self.next_node_id)
self.log.info('Reached node %s', self.next_node_id)
if self.next_node['name'] == 'Stairwell':
self.audio.prompt_stairs()
elif self.next_node['name'].lower().find('door') != -1:
self.audio.prompt_door()
self.next_node_idx += 1
if self.next_node_idx == len(self.path):
self.navi_chunk_finished = True
self.log.info('Reached destination node')
else:
if self.next_node['name'].lower().find('office') != -1 or \
self.next_node['name'].lower().find('room') != -1:
self.audio.prompt_door()
self.log.info('Navigating to node #%s %s',
self.next_node_id, self.next_node['name'])
def start(self):
self.cam.start()
self._wait_for_origin_and_destination()
self._generate_chunks()
while len(self.navi_chunks) != 0:
self._prepare_for_next_navi_chunk()
self._get_map()
self._generate_path()
self._wait_for_chunk_to_finish()
self.navi_chunks.pop(0)
self.audio.prompt(PromptDirn.end)
