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: