EXPERIMENTS = 1
SIZE = 26
seeds = np.linspace(76842153, 1123581321, 100)
sensors = ["t"]
drones = [SimpleAgent(sensors)]
env = Env(map_path2yaml="E:/ETSI/Proyecto/data/Map/Simple/map.yaml")
env.add_new_map(sensors, file=98)
plt.style.use("seaborn")
# plt.subplot(241)
plt.subplot(131)
plt.imshow(env.render_maps()["t"], origin='lower', cmap='inferno')
CS = plt.contour(env.render_maps()["t"],
colors=('gray', 'gray', 'gray', 'k', 'k', 'k', 'k'),
alpha=0.6,
linewidths=1.0)
plt.clabel(CS, inline=1, fontsize=10)
plt.title("Ground Truth")
# plt.show(block=False)
# plt.pause(0.01)
mses = {"RBF": [], "RBF_N": [], "RQ": []}
with open('/data/Databases/CSV/nans.npy', 'rb') as g:
nans = np.load(g)
candidates = [[133, 832], [117, 792], [190, 846], [270, 761], [422, 810],
[556, 871], [663, 761], [741, 622], [804, 469], [868, 314],
class Simulator(object):
def __init__(self, map_path2yaml, agents, main_sensor, saving=False, test_name="", acq=-1):
"""
Simulator(map_path2yaml, agents, render)
Returns a new Simulator.
Parameters
----------
map_path2yaml : string
Absolute path to map yaml archive , e.g., ``"C:/data/yaml"``.
agents : list of bin.Environment.simple_agent
Agents
Returns
-------
out : Simulator
See Also
--------
Examples
--------
"""
self.saving = saving
self.environment = Env(map_path2yaml=map_path2yaml)
self.agents = agents
self.sensors = set()
self.init_maps()
self.load_envs_into_agents()
if main_sensor in self.sensors:
self.main_sensor = main_sensor
for agent in self.agents:
agent.randomize_pos()
# agent.randomize_pos()
# agent.randomize_pos()
# agent.randomize_pos()
self.sender = Sender()
if acq == 1:
acq = "LD"
elif acq == 2:
acq = "LU"
elif acq == 3:
acq = "RD"
elif acq == 4:
acq = "RU"
else:
raise Exception
self.coordinator = Coordinator(self.agents, self.environment.grid, self.main_sensor, acq)
# self.coordinator.acquisition = "maxvalue_entropy_search"
self.sender.send_new_acq_msg(self.coordinator.acquisition)
for agent in self.agents:
agent.next_pose = self.coordinator.generate_new_goal(pose=agent.pose)
agent.step()
agent.distance_travelled = 0
self.sender.send_new_drone_msg(agent.pose)
self.use_cih_as_initial_points = True
if self.use_cih_as_initial_points:
import numpy as np
reads = [
[np.array([785, 757]), self.environment.maps["t"][757, 785]], # CSNB (757, 785)
# [np.array([492, 443]), self.environment.maps["t"][443, 492]], # YVY (443, 492)
[np.array([75, 872]), self.environment.maps["t"][872, 75]], # PMAregua (872, 75)
self.agents[0].read()
]
print(reads)
self.sender.send_new_sensor_msg(str(reads[0][0][0]) + "," + str(reads[0][0][1]) + "," + str(reads[0][1]))
self.sender.send_new_sensor_msg(str(reads[1][0][0]) + "," + str(reads[1][0][1]) + "," + str(reads[1][1]))
self.sender.send_new_sensor_msg(str(reads[2][0][0]) + "," + str(reads[2][0][1]) + "," + str(reads[2][1]))
else:
reads = [agent.read() for agent in self.agents]
for i in range(2):
self.agents[0].randomize_pos()
reads.append(self.agents[0].read())
self.sender.send_new_sensor_msg(str(reads[0][0][0]) + "," + str(reads[0][0][1]) + "," + str(reads[0][1]))
self.sender.send_new_sensor_msg(str(reads[1][0][0]) + "," + str(reads[1][0][1]) + "," + str(reads[1][1]))
self.sender.send_new_sensor_msg(str(reads[2][0][0]) + "," + str(reads[2][0][1]) + "," + str(reads[2][1]))
self.coordinator.initialize_data_gpr(reads)
plt.imshow(self.environment.render_maps()["t"], origin='lower', cmap='inferno')
plt.colorbar(orientation='vertical')
CS = plt.contour(self.environment.render_maps()["t"], colors=('gray', 'gray', 'gray', 'k', 'k', 'k', 'k'),
alpha=0.6, linewidths=1.0)
plt.clabel(CS, inline=1, fontsize=10)
plt.title("Ground Truth")
plt.xlabel("x")
plt.ylabel("y")
plt.draw()
# plt.pause(0.0001)
plt.show(block=True)
if saving:
self.f = open("E:/ETSI/Proyecto/results/csv_results/{}_{}.csv".format(test_name, int(time.time())), "a")
self.f.write("kernel,acq,masked\n")
self.f.write(str(
"LM,{},{}\n".format(self.coordinator.acquisition, self.coordinator.acq_method)))
self.f.write("step,mse,t_dist\n")
mse = self.coordinator.get_mse(self.environment.maps['t'].T.flatten())
self.f.write("{},{},{}\n".format(0, mse, self.agents[0].distance_travelled))
def init_maps(self):
if isinstance(self.agents, sa.SimpleAgent):
[self.sensors.add(sensor) for sensor in self.agents.sensors]
elif isinstance(self.agents, list) and isinstance(self.agents[0], sa.SimpleAgent) or \
isinstance(self.agents, list) and isinstance(self.agents[0], ppa.SimpleAgent):
for agent in self.agents:
[self.sensors.add(sensor) for sensor in agent.sensors]
self.environment.add_new_map(self.sensors)
def load_envs_into_agents(self):
for agent in self.agents:
agent.set_agent_env(self.environment)
def run_simulation(self):
imax = 20
i = 0
while i < imax:
# if not self.sender.should_update():
# plt.pause(0.5)
# continue
if isinstance(self.agents, sa.SimpleAgent):
self.agents.next_pose = self.coordinator.generate_new_goal()
self.agents.step()
self.coordinator.add_data(self.agents.read())
self.coordinator.fit_data()
elif isinstance(self.agents, list) and isinstance(self.agents[0], sa.SimpleAgent) or \
isinstance(self.agents, list) and isinstance(self.agents[0], ppa.SimpleAgent):
for agent in self.agents:
if agent.reached_pose():
agent.next_pose = self.coordinator.generate_new_goal(pose=agent.pose)
if agent.step():
time.sleep(1)
read = agent.read()
self.coordinator.add_data(read)
self.sender.send_new_sensor_msg(
str(read[0][0]) + "," + str(read[0][1]) + "," + str(read[1]))
self.sender.send_new_drone_msg(agent.pose)
self.coordinator.fit_data()
# dataaa = np.exp(-cdist([agent.pose[:2]],
# self.coordinator.all_vector_pos) / 150).reshape(1000, 1500).T
# plt.imshow(dataaa,
# origin='lower', cmap='YlGn_r')
# if i == 0:
# plt.colorbar(orientation='vertical')
else:
i -= 1
mse = self.coordinator.get_mse(self.environment.maps['t'].T.flatten())
# plt.title("MSE is {}".format(mse))
# plt.draw()
# plt.pause(0.0001)
i += 1
# if self.agents[0].distance_travelled > 1500:
# print(mse)
# import numpy as np
# # with open('E:/ETSI/Proyecto/data/Databases/numpy_files/best_lm.npy', 'wb') as g:
# # np.save(g, self.coordinator.surrogate().reshape((1000, 1500)).T)
# plt.show(block=True)
if self.saving:
self.f.write("{},{},{}\n".format(i, mse, self.agents[0].distance_travelled))
print("done")
self.sender.client.disconnect()
if self.saving:
self.f.close()
class Simulator(object):
def __init__(self, map_path2yaml, agents, main_sensor, render=False):
"""
Simulator(map_path2yaml, agents, render)
Returns a new Simulator.
Parameters
----------
map_path2yaml : string
Absolute path to map yaml archive , e.g., ``"C:/data/yaml"``.
agents : bin.Environment.simple_agent
Agents
Returns
-------
out : Simulator
See Also
--------
Examples
--------
"""
self.environment = Env(map_path2yaml=map_path2yaml)
self.agents = agents
self.render = render
self.sensors = set()
self.init_maps()
self.load_envs_into_agents()
if main_sensor in self.sensors:
self.main_sensor = main_sensor
self.agents.randomize_pos()
self.coordinator = Coordinator(self.agents, self.environment.grid,
self.main_sensor)
self.coordinator.initialize_data_gpr(self.agents.read())
self.agents.randomize_pos(near=True)
self.coordinator.add_data(self.agents.read())
self.coordinator.fit_data()
if render:
self.render = render
self.GUI = GUI(self.environment.maps)
else:
self.render = False
def init_maps(self):
if isinstance(self.agents, sa.SimpleAgent):
[self.sensors.add(sensor) for sensor in self.agents.sensors]
elif isinstance(self.agents, list) and isinstance(
self.agents[0], sa.SimpleAgent):
for agent in self.agents:
[self.sensors.add(sensor) for sensor in agent.sensors]
self.environment.add_new_map(self.sensors)
def load_envs_into_agents(self):
if isinstance(self.agents, sa.SimpleAgent):
self.agents.set_agent_env(self.environment)
elif isinstance(self.agents, list) and isinstance(
self.agents[0], sa.SimpleAgent):
for agent in self.agents:
agent.set_agent_env(self.environment)
def export_maps(self, sensors=None, extension='png', render=False):
data = self.environment.render_maps(sensors)
mu, std, sensor_name = self.coordinator.surrogate(return_std=True,
return_sensor=True)
# self.coordinator.gp.predict(self.coordinator.vector_pos, return_std=True)
img.imsave(
"E:/ETSI/Proyecto/results/Map/{}_{}.{}".format(
datetime.datetime.now().timestamp(),
"{}_u_gp".format(sensor_name), extension),
1.96 * std.reshape(self.GUI.shape[1], self.GUI.shape[0]).T)
img.imsave(
"E:/ETSI/Proyecto/results/Map/{}_{}.{}".format(
datetime.datetime.now().timestamp(),
"{}_gp".format(sensor_name), extension),
mu.reshape(self.GUI.shape[1], self.GUI.shape[0]).T)
for key, _map in data.items():
if _map is not None:
img.imsave(
"E:/ETSI/Proyecto/results/Map/{}_{}.{}".format(
datetime.datetime.now().timestamp(), key, extension),
_map)
else:
print("sensor {} not saved".format(key))
def run_simulation(self):
imax = 10
data = self.environment.render_maps()
for i in range(imax):
if isinstance(self.agents, sa.SimpleAgent):
self.agents.next_pose = next_pos
self.agents.step()
self.coordinator.add_data(self.agents.read())
self.coordinator.fit_data()
elif isinstance(self.agents, list) and isinstance(
self.agents[0], sa.SimpleAgent):
for agent in self.agents:
agent.step()
if self.render:
mu, std, sensor_name = self.coordinator.surrogate(
return_std=True, return_sensor=True)
# titles = {"MSE": self.coordinator.get_mse(self.environment.maps["temp"].flatten())}
# print(titles)
# if isinstance(self.agents, sa.SimpleAgent):
data["{} gp".format(sensor_name)] = mu
data["{} gp un".format(sensor_name)] = std
self.GUI.observe_maps(data)
self.GUI.export_maps()
