def create_dijkstras_map(state_: State):
result_dict = defaultdict(int)
for goal_location in state_.goal_positions.keys():
preprocessing_current_state = State(state_)
strategy = StrategyBFS()
strategy.reset_strategy()
preprocessing_current_state._dijkstras_location = goal_location
strategy.add_to_frontier(preprocessing_current_state)
iterations = 0
while True:
if iterations == 1000:
if testing:
print(f"dijkstras {strategy.search_status()}",
file=sys.stderr,
flush=True)
iterations = 0
if memory.get_usage() > memory.max_usage:
if testing:
print('Maximum memory usage exceeded.',
file=sys.stderr,
flush=True)
raise Exception('Maximum mem used')
if strategy.frontier_empty():
# print('Done with dijkstras', file=sys.stderr, flush=True)
break
leaf = strategy.get_and_remove_leaf()
# Add element to dict
result_dict[(goal_location, leaf._dijkstras_location)] = leaf.g
strategy.add_to_explored(leaf)
for child_state in leaf.get_children_dijkstras(
): # The list of expanded states is shuffled randomly; see state.py.
if not strategy.is_explored(
child_state) and not strategy.in_frontier(child_state):
strategy.add_to_frontier(child_state)
iterations += 1
return result_dict
def search_conflict_bfs_not_in_list(self,
world_state: 'State',
agent_collision_internal_id,
agent_collision_box,
box_id,
coordinates: list,
move_action_allowed=True):
'''
This search method uses bfs to find the first location the agent can move to without being in the list
of coordinate
:param agent_collision_box: id of box involved in collision
:param agent_collision_internal_id: id of agent involved in collision
:param world_state: world_state with
:param coordinates: list of coordinates that the agent is not allowed to be in
:return: None (update the plan of the agent to not interfer with the coordinates give
'''
d = {
'world_state': world_state,
'agent_collision_internal_id': agent_collision_internal_id,
'agent_collision_box': agent_collision_box,
'box_id': box_id,
'coordinates': coordinates,
'move_action_allowed': move_action_allowed
}
#
self.world_state = State(world_state)
if box_id is None:
move_action_allowed = True
else:
# fix for missing boxes in search
self.current_box_id = box_id
self.goal_job_id = None
# test case
if (box_id is not None) and (utils.cityblock_distance(
utils._get_agt_loc(self.world_state, self.agent_char),
utils._get_box_loc(self.world_state, box_id)) > 1):
locc_agt = utils._get_agt_loc(self.world_state, self.agent_char)
locc_box = utils._get_box_loc(self.world_state, box_id)
box_id = None
move_action_allowed = True
#If agt is asked to move out of someones plan, then include this someone in the planinng
if agent_collision_internal_id is not None:
self.world_state.redirecter_search = True
removed_dict = {
k: v
for k, v in self.world_state.agents.items()
if (v[0][1] == self.agent_char) or (
v[0][2] == agent_collision_internal_id)
}
self.world_state.agents = defaultdict(list, removed_dict)
removed_dict = {
k: v
for k, v in self.world_state.boxes.items()
if (v[0][2] == self.current_box_id) or (
v[0][2] == agent_collision_box)
}
self.world_state.boxes = defaultdict(list, removed_dict)
strategy = StrategyBFS()
# In case there has been a previous search we need to clear the elements in the strategy object
strategy.reset_strategy()
# Fix error with state not knowing which box is allowed to be moved
self.world_state.sub_goal_box = box_id
strategy.add_to_frontier(state=self.world_state)
iterations = 0
_counter = 0
while True:
if _counter == _cfg.max_search_depth:
return False
if iterations == 1000:
if _cfg.testing:
print(f"bfs not in list {strategy.search_status()}",
file=sys.stderr,
flush=True)
iterations = 0
if memory.get_usage() > memory.max_usage:
if _cfg.testing:
print('Maximum memory usage exceeded.',
file=sys.stderr,
flush=True)
return None
if strategy.frontier_empty():
if _cfg.testing:
print('Empty frontier BFS NOT IN LIST',
file=sys.stderr,
flush=True)
print(f'{self.agent_char}', file=sys.stderr, flush=True)
'''
# TODO: Could not find a location where agent is not "in the way" - return something that triggers
the other collision object to move out of the way
'''
return False
leaf = strategy.get_and_remove_leaf()
agt_loc = utils._get_agt_loc(leaf, self.agent_char)
if box_id is not None:
box_loc = utils._get_box_loc(leaf, self.current_box_id)
if (box_id is None) and (agt_loc not in coordinates):
self._reset_plan()
self._convert_plan_to_action_list(leaf.extract_plan())
return True
#break
else:
if agt_loc not in coordinates and box_loc not in coordinates:
self._reset_plan()
self._convert_plan_to_action_list(leaf.extract_plan())
return True
strategy.add_to_explored(leaf)
x = strategy.explored.pop()
strategy.explored.add(x)
for child_state in leaf.get_children(
self.agent_char, move_allowed=move_action_allowed
): # The list of expanded states is shuffled randomly;
if not strategy.is_explored(
child_state) and not strategy.in_frontier(child_state):
strategy.add_to_frontier(child_state)
iterations += 1
_counter += 1