count_plans += 1
if sol:
path = sol['path']
actions = sol['actions']
actions_prev = np.copy(actions[1:])
else:
actions = actions_prev.copy()
S = []
while not len(S) or p_max == 1.0:
try:
a = actions.pop(0)
except:
print("End of plan.")
break
x_current = E.action(a, loc=x_current)
print("a: ", a, x_current)
P.append(E.get_max_prob())
# Sense cell
S = E.sense(x_current)
print("Senses %d obstacles." % len(S))
E.plot(path_map=True,
heat_map=False,
stop=False,
external_path=np.array(path))
actions_prev = actions.copy()
# Update belief map
for s in S:
