def fix_g(self, old_state, new_parent):
state = self.tree.get_node(old_state.__str__())
old_parent = self.tree.get_node(state.predecessor(
self.tree.identifier))
new_parent = self.tree.get_node(new_parent.__str__())
if self.seen_comparison(new_parent, old_parent):
self.tree.move_node(state.identifier, new_parent.identifier)
new_g = new_parent.data.g + \
Utils.n_dim_distance(self.tree.get_node(state.identifier).data.pos, new_parent.data.pos)
self.tree.get_node(state.identifier).data.f = self.tree.get_node(state.identifier).data.f - \
self.tree.get_node(state.identifier).data.g + new_g
self.tree.get_node(state.identifier).data.g = new_g
for index, data in enumerate(self.open_list):
if np.all(data.pos == state.data.pos):
self.open_list = np.delete(self.open_list, index)
self.insert_to_open_list(
self.tree.get_node(state.identifier).data)
break
def expend(self, state):
move_index = np.zeros(self.number_of_agent).astype(int)
for i in range(LOS**number_of_agent):
for j in range(number_of_agent):
i, index = divmod(i, LOS)
move_index[j] = index
neighbor = self.world.get_one_neighbor(state, move_index)
if self.world.in_bund(neighbor) and self.world.is_obstical(
neighbor):
new_g = mwrp.tree.get_node(
state.__str__()).data.g + Utils.n_dim_distance(
state, neighbor)
if not self.tree.contains(neighbor.__str__()):
h = self.heuristic(state)
new_node = Node(neighbor, new_g, new_g + h)
self.tree.create_node(neighbor.__str__(),
neighbor.__str__(),
parent=(state.__str__()),
data=new_node)
self.insert_to_open_list(new_node)
else:
if new_g < self.tree.get_node(neighbor.__str__()).data.g:
self.fix_g(neighbor, state)
self.move_from_open_to_close()
def create_wachers(self):
all_free_cell = set(
map(tuple,
np.asarray(np.where(self.grid_map == 0)).T))
for cell in all_free_cell:
tmp_set = self.get_fov(cell)
#if cell == (7,13):
#Utils.print_fov(self.grid_map,tmp_set,cell)
self.dict_fov[cell] = tmp_set
for wahers in tmp_set:
if wahers != cell:
if wahers in self.dict_wachers:
self.dict_wachers[wahers] = self.dict_wachers[
wahers].union(Utils.map_to_sets(cell))
else:
self.dict_wachers[wahers] = Utils.map_to_sets(cell)
def expend(self):
t = time()
old_state = self.pop_open_list()
self.expend_node += 1
self.H_expend += old_state.f
if self.goal_test(old_state.unseen):
return old_state
# Utils.print_map(self.world)
Utils.print_all_whacers(self.world, [[(2, 1)], [(1, 3)]])
for new_state in itertools.product(*self.get_all_frontire(old_state)):
if new_state != old_state.location: # and set(new_state).__len__()==self.number_of_agent:
sorted_new_state, sorted_indexing = Utils.sort_list(new_state)
dead_list = self.get_dead_list(old_state, new_state,
sorted_indexing)
seen_state = old_state.unseen - self.world.get_all_seen(
sorted_new_state)
new_node = Node(
old_state, sorted_new_state, seen_state, dead_list,
self.get_cost(new_state, old_state, sorted_indexing))
self.insert_to_open_list(new_node)
#print(time() - t)
# for state_index in range(LOS ** (self.number_of_agent)):
#
# new_state , moving_status = self.get_new_state(old_state, state_index)
#
# if self.world.is_valid_node(new_state, old_state,moving_status):
#
# seen_state = old_state.unseen - self.world.get_all_seen(new_state)
# dead_list=old_state.dead_agent[:]
# for i in range(self.number_of_agent):
# if moving_status[i]==0 and i not in dead_list:
# dead_list.append(i)
# new_node = Node(old_state, new_state, seen_state,dead_list, self.get_cost(old_state), 0)
#
# self.insert_to_open_list(new_node)
# #print(time()-t)
return False
def print_path(self, gole_node, see_agent_walk):
all_path = self.get_path(gole_node)
tmp_location = []
for cell in all_path:
# print(f'L = {cell.location} \t h = {cell.heuristics} ')
tmp_location.append(cell.location)
tmp_word = np.copy(self.world.grid_map)
for k in cell.unseen:
tmp_word[k] = 2
for j in cell.location:
tmp_word[j] = 3
if see_agent_walk:
plt.figure(1)
plt.pcolormesh(tmp_word, edgecolors='black', linewidth=0.01)
plt.gca().set_aspect('equal')
plt.gca().set_ylim(plt.gca().get_ylim()[::-1])
plt.draw()
plt.pause(0.001)
plt.clf()
sleep(0.5)
plt.close('all')
Utils.print_all_whacers(self.world, tmp_location)
plt.clf()
sleep(0.5)
plt.close('all')
Utils.print_all_whacers(self.world, tmp_location)
import csv
from alive_progress import alive_bar
if __name__ == '__main__':
map_type = 'maze_11_11_'
map_config = './config/{}config.csv'.format(map_type)
#map_config='./config/for_jornal.csv'
#row_map = np.genfromtxt(map_config, delimiter=',', case_sensitive=True)
row_map = Utils.convert_map(map_config)
LOS = 4 + 1
all_free = np.transpose(np.where(np.array(row_map) == 0))
from datetime import datetime
pivot = [5]
exp_number = 72
huristics_exp = [0, 1, 2]
loop_number_of_agent = [2, 4, 6]
# for ii in range(100):
# start_pos = tuple(tuple(all_free[randint(0,all_free.__len__()-1)]) for f in range(max_number_of_agent))
# print(start_pos)
start_in = 0