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