def compute_dist_and_path_tables(self):
# Find the shortest distance of each state in the product to the final states
dist_table = dict()
path_table = dict()
for node in self.global_pa.g.nodes():
dist_table[node] = float('inf') # initialize to inf
path_table[node] = None
dists, paths = dijkstra_to_all(self.global_pa.g, node, degen_paths=True, weight_key='weight') # returns 0 for node=final
for final in self.global_pa.final:
if final in dists and dists[final] < dist_table[node]:
dist_table[node] = dists[final]
path_table[node] = paths[final]
return (dist_table, path_table)
def compute_dist_and_path_tables(self):
# Find the shortest distance of each state in the product to the final states
dist_table = dict()
path_table = dict()
for node in self.global_pa.g.nodes():
dist_table[node] = float('inf') # initialize to inf
path_table[node] = None
dists, paths = dijkstra_to_all(self.global_pa.g, node, degen_paths=True, weight_key='weight') # returns 0 for node=final
for final in self.global_pa.final:
if final in dists and dists[final] < dist_table[node]:
dist_table[node] = dists[final]
path_table[node] = paths[final]
return (dist_table, path_table)
def next_command(self):
# Construct the local TS
local_ts = self.construct_local_ts()
local_ts_state = local_ts.init.keys()[0]
# Initialize vars to hold optimal vals
# (path_star is for debugging purposes)
d_star, cell_next_star, target_cell_star, g_next_star, path_star = float('inf'), None, None, None, None
# Get the set of sensed local requests
local_reqs = reduce(lambda a,b: a|b, [local_ts.g.node[cell]['prop'] for cell in local_ts.g], set([]))
logger.debug('Sensed local requests: %s' % local_reqs)
# Find the set of requests that can be serviced
# (D_service in alg.)
enabled_reqs = set([])
for _, _, d in self.local_fsa.g.out_edges_iter((self.local_fsa_state,), data=True):
enabled_reqs = enabled_reqs | d['input']
serviceable_reqs = local_reqs & enabled_reqs
# Find the cells with static and dynamic requests
global_req_cells = reduce(lambda a,b: a|b, [set([q]) for q in local_ts.g if q in self.env.global_reqs], set([]))
local_req_cells = reduce(lambda a,b: a|b, [set([q]) for q in local_ts.g if local_ts.g.node[q]['prop']], set([]))
if not serviceable_reqs:
# Consider all neighbors of our current state in the global product automaton
for cur, neigh in self.global_pa.g.out_edges_iter(self.global_pa_state):
if neigh[0] in local_ts.g:
# Avoid all cells with global and local requests
# except the cell corresponding to neigh
avoid_cells = (global_req_cells | local_req_cells) - {neigh[0]}
# If neigh[0] is in local_ts, it is our target
target_cells = {neigh[0]}
else:
# Avoid all cells with global and local requests
avoid_cells = global_req_cells | local_req_cells
# Reach boundary cells
target_cells = set([])
x_min, y_min = self.quad.get_sensing_cell_global_coords((0,0))
x_max, y_max = self.quad.get_sensing_cell_global_coords((self.quad.sensing_range-1,self.quad.sensing_range-1))
for local_cell in local_ts.g.nodes_iter():
x, y = local_cell
if (x == x_min or x == x_max or y == y_min or y == y_max):
target_cells.add(local_cell)
# Remove those cells that we have to avoid
target_cells = target_cells - avoid_cells
# Set incoming edge weights of avoid_cells to inf
for u, v, k in local_ts.g.in_edges_iter(avoid_cells, keys=True):
local_ts.g[u][v][k]['weight'] = float('inf')
# Find shortest paths
dists, paths = dijkstra_to_all(local_ts.g, local_ts_state)
# Restore incoming edge weights of avoided cells
for u, v, k in local_ts.g.in_edges_iter(avoid_cells, keys=True):
local_ts.g[u][v][k]['weight'] = 1
# Plan for each cell in target_cells while avoiding the cells in avoid_cells
for target_cell in target_cells:
# We also consider the length of the local path
d_plan = abs(target_cell[0]-neigh[0][0]) + abs(target_cell[1]-neigh[0][1])
d_plan += self.dist_table[neigh]
d_plan += dists[target_cell]
if d_plan < d_star or (d_plan == d_star and target_cell_star and (target_cell[0] < target_cell_star[0] or (target_cell[0] == target_cell_star[0] and target_cell[1] > target_cell_star[1]))):
d_star = d_plan
g_next_star = neigh
cell_next_star = paths[target_cell][1]
path_star = paths[target_cell]
target_cell_star = target_cell
else:
# Drive the vehicle to the closest local request with the highest
# priority while avoiding all other sensed local requests and
# global requests within the sensing range
# Find the maximum priority request (min prio number)
max_prio = min([self.prio[req] for req in serviceable_reqs])
# Find the set of target requests
target_reqs = set([req for req in serviceable_reqs if self.prio[req] == max_prio])
target_cells = set([cell for cell in local_ts.g if local_ts.g.node[cell]['prop'] & target_reqs])
logger.debug('Will service local requests: %s at cells %s' % (target_reqs, target_cells))
# Avoid all cells with global and local requests
# except those in target_cells
avoid_cells = (global_req_cells | local_req_cells) - target_cells
logger.debug('Cells to avoid: %s' % avoid_cells)
# Set incoming edge weights of avoid_cells to inf
for u, v, k in local_ts.g.in_edges_iter(avoid_cells, keys=True):
local_ts.g[u][v][k]['weight'] = float('inf')
# Find shortest paths
dists, paths = dijkstra_to_all(local_ts.g, local_ts_state)
# Restore incoming edge weights of avoided cells
for u, v, k in local_ts.g.in_edges_iter(avoid_cells, keys=True):
local_ts.g[u][v][k]['weight'] = 1
# Plan for each cell in target_cells while avoiding the cells in avoid_cells
for target_cell in target_cells:
if dists[target_cell] == float('inf'):
# No path to this target_cell
continue
d_plan = dists[target_cell]
if d_plan < d_star:
d_star = d_plan
g_next_star = None
cell_next_star = paths[target_cell][1]
path_star = paths[target_cell]
assert d_star != float('inf'), 'Could not find a feasible local plan'
logger.debug('Path to target: %s, cell_next_star: %s' % (path_star, cell_next_star))
# Find the command to reach cell_next_star
assert len(local_ts.g[local_ts_state][cell_next_star]) == 1, 'Local TS cannot have parallel edges'
control_star = local_ts.g[local_ts_state][cell_next_star][0]['control']
# Update global NBA state as necessary
if g_next_star and g_next_star[0] == cell_next_star:
self.global_pa_state = g_next_star
logger.debug('Updated global product automaton state to: %s' % (g_next_star,))
# Update local FSA state as necessary
found_next_local_fsa_state = False
next_local_req = local_ts.g.node[cell_next_star]['prop']
if next_local_req:
for _, next_local_fsa_state, d in self.local_fsa.g.out_edges_iter((self.local_fsa_state,), data=True):
if d['input'] == next_local_req:
self.local_fsa_state = next_local_fsa_state
found_next_local_fsa_state = True
break
assert found_next_local_fsa_state, 'Local FSA does not have a transition for %s from its current state %s' % (next_local_req, self.local_fsa_state)
# Turn off the serviced request as necessary
if next_local_req:
self.env.local_reqs[cell_next_star]['on'] = False
return (control_star, path_star)
def next_command(self):
# Construct the local TS
local_ts = self.construct_local_ts()
local_ts_state = list(local_ts.init.keys())[0]
# Initialize vars to hold optimal vals
# (path_star is for debugging purposes)
d_star, cell_next_star, target_cell_star, g_next_star, path_star = float(
'inf'), None, None, None, None
# Get the set of sensed local requests
local_reqs = reduce(
lambda a, b: a | b,
[local_ts.g.node[cell]['prop'] for cell in local_ts.g], set([]))
logger.debug('Sensed local requests: %s' % local_reqs)
# Find the set of requests that can be serviced
# (D_service in alg.)
enabled_reqs = set([])
for _, _, d in self.local_fsa.g.out_edges_iter(
(self.local_fsa_state, ), data=True):
enabled_reqs = enabled_reqs | d['input']
serviceable_reqs = local_reqs & enabled_reqs
# Find the cells with static and dynamic requests
global_req_cells = reduce(
lambda a, b: a | b,
[set([q])
for q in local_ts.g if q in self.env.global_reqs], set([]))
local_req_cells = reduce(
lambda a, b: a | b,
[set([q]) for q in local_ts.g if local_ts.g.node[q]['prop']],
set([]))
if not serviceable_reqs:
# Consider all neighbors of our current state in the global product automaton
for cur, neigh in self.global_pa.g.out_edges_iter(
self.global_pa_state):
if neigh[0] in local_ts.g:
# Avoid all cells with global and local requests
# except the cell corresponding to neigh
avoid_cells = (global_req_cells
| local_req_cells) - {neigh[0]}
# If neigh[0] is in local_ts, it is our target
target_cells = {neigh[0]}
else:
# Avoid all cells with global and local requests
avoid_cells = global_req_cells | local_req_cells
# Reach boundary cells
target_cells = set([])
x_min, y_min = self.quad.get_sensing_cell_global_coords(
(0, 0))
x_max, y_max = self.quad.get_sensing_cell_global_coords(
(self.quad.sensing_range - 1,
self.quad.sensing_range - 1))
for local_cell in local_ts.g.nodes_iter():
x, y = local_cell
if (x == x_min or x == x_max or y == y_min
or y == y_max):
target_cells.add(local_cell)
# Remove those cells that we have to avoid
target_cells = target_cells - avoid_cells
# Set incoming edge weights of avoid_cells to inf
for u, v, k in local_ts.g.in_edges_iter(avoid_cells,
keys=True):
local_ts.g[u][v][k]['weight'] = float('inf')
# Find shortest paths
dists, paths = dijkstra_to_all(local_ts.g, local_ts_state)
# Restore incoming edge weights of avoided cells
for u, v, k in local_ts.g.in_edges_iter(avoid_cells,
keys=True):
local_ts.g[u][v][k]['weight'] = 1
# Plan for each cell in target_cells while avoiding the cells in avoid_cells
for target_cell in target_cells:
# We also consider the length of the local path
d_plan = abs(target_cell[0] -
neigh[0][0]) + abs(target_cell[1] -
neigh[0][1])
d_plan += self.dist_table[neigh]
d_plan += dists[target_cell]
if d_plan < d_star or (
d_plan == d_star and target_cell_star and
(target_cell[0] < target_cell_star[0] or
(target_cell[0] == target_cell_star[0]
and target_cell[1] > target_cell_star[1]))):
d_star = d_plan
g_next_star = neigh
cell_next_star = paths[target_cell][1]
path_star = paths[target_cell]
target_cell_star = target_cell
else:
# Drive the vehicle to the closest local request with the highest
# priority while avoiding all other sensed local requests and
# global requests within the sensing range
# Find the maximum priority request (min prio number)
max_prio = min([self.prio[req] for req in serviceable_reqs])
# Find the set of target requests
target_reqs = set([
req for req in serviceable_reqs if self.prio[req] == max_prio
])
target_cells = set([
cell for cell in local_ts.g
if local_ts.g.node[cell]['prop'] & target_reqs
])
logger.debug('Will service local requests: %s at cells %s' %
(target_reqs, target_cells))
# Avoid all cells with global and local requests
# except those in target_cells
avoid_cells = (global_req_cells | local_req_cells) - target_cells
logger.debug('Cells to avoid: %s' % avoid_cells)
# Set incoming edge weights of avoid_cells to inf
for u, v, k in local_ts.g.in_edges_iter(avoid_cells, keys=True):
local_ts.g[u][v][k]['weight'] = float('inf')
# Find shortest paths
dists, paths = dijkstra_to_all(local_ts.g, local_ts_state)
# Restore incoming edge weights of avoided cells
for u, v, k in local_ts.g.in_edges_iter(avoid_cells, keys=True):
local_ts.g[u][v][k]['weight'] = 1
# Plan for each cell in target_cells while avoiding the cells in avoid_cells
for target_cell in target_cells:
if dists[target_cell] == float('inf'):
# No path to this target_cell
continue
d_plan = dists[target_cell]
if d_plan < d_star:
d_star = d_plan
g_next_star = None
cell_next_star = paths[target_cell][1]
path_star = paths[target_cell]
assert d_star != float('inf'), 'Could not find a feasible local plan'
logger.debug('Path to target: %s, cell_next_star: %s' %
(path_star, cell_next_star))
# Find the command to reach cell_next_star
assert len(
local_ts.g[local_ts_state]
[cell_next_star]) == 1, 'Local TS cannot have parallel edges'
control_star = local_ts.g[local_ts_state][cell_next_star][0]['control']
# Update global NBA state as necessary
if g_next_star and g_next_star[0] == cell_next_star:
self.global_pa_state = g_next_star
logger.debug('Updated global product automaton state to: %s' %
(g_next_star, ))
# Update local FSA state as necessary
found_next_local_fsa_state = False
next_local_req = local_ts.g.node[cell_next_star]['prop']
if next_local_req:
for _, next_local_fsa_state, d in self.local_fsa.g.out_edges_iter(
(self.local_fsa_state, ), data=True):
if d['input'] == next_local_req:
self.local_fsa_state = next_local_fsa_state
found_next_local_fsa_state = True
break
assert found_next_local_fsa_state, 'Local FSA does not have a transition for %s from its current state %s' % (
next_local_req, self.local_fsa_state)
# Turn off the serviced request as necessary
if next_local_req:
self.env.local_reqs[cell_next_star]['on'] = False
return (control_star, path_star)