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()
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))
class Simulator(object):
def __init__(self, map_path2yaml, agents: list, main_sensor, saving=False, test_name="", acq="gaussian_sei",
acq_mod="normal", file=0, initial_pos="circle"):
"""
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 sppa.SimpleAgent
Agents
Returns
-------
out : Simulator
See Also
--------
Examples
--------
"""
self.saving = saving
self.file_no = file
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
self.sender = Sender()
# if initial_pos == "none":
# print("random initial positions")
# elif initial_pos == "circle":
# print("circle initial positions")
# initial_positions = get_init_pos4(n=len(agents))
# elif initial_pos == "cvt":
# print("cvt initial positions")
initial_positions = get_init_pos4(n=len(agents), map_data=self.environment.grid, expand=True)
for agent in self.agents:
aux_f = agent.position_flag
agent.position_flag = False
if initial_pos == "circle":
agent.pose, initial_positions = initial_positions[0, :], initial_positions[1:, :]
else:
agent.randomize_pos()
agent.randomize_pos()
# agent.randomize_pos(near=False)
agent.next_pose = deepcopy(agent.pose)
agent.position_flag = aux_f
self.sender.send_new_drone_msg(agent.pose, agent.drone_id)
self.sender.send_new_goal_msg(agent.next_pose, agent.drone_id)
self.coordinator = Coordinator(self.environment.grid, self.main_sensor, acq=acq, acq_mod=acq_mod)
self.sender.send_new_acq_msg(self.coordinator.acquisition)
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)
]
[reads.append(read.read()) for read in self.agents]
for [read, my_id] in zip(reads, [0, 1, 2, 3, 4]):
self.sender.send_new_sensor_msg(str(read[0][0]) + "," + str(read[0][1]) + "," + str(read[1]),
_id=my_id)
self.sender.send_new_drone_msg(self.agents[my_id].pose, my_id)
plt.pause(0.01)
# plt.plot(reads[0][0][0], reads[0][0][1], '^y', markersize=12, label="Previous Positions")
self.coordinator.initialize_data_gpr(reads)
# from copy import copy
# import matplotlib.cm as cm
# current_cmap = copy(cm.get_cmap("inferno"))
# current_cmap.set_bad(color="#eaeaf2")
#
# plt.imshow(self.environment.render_maps()["t"], origin='lower', cmap=current_cmap) # YlGn_r
# cbar = plt.colorbar(orientation='vertical')
# cbar.ax.tick_params(labelsize=20)
# 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("Mask", fontsize=30)
# plt.xlabel("x", fontsize=20)
# plt.ylabel("y", fontsize=20)
# plt.xticks(fontsize=20)
# plt.yticks(fontsize=20)
# plt.draw()
# plt.pause(0.0001)
# plt.show(block=True)
if saving:
self.f = open(
"E:/ETSI/Proyecto/results/{}_{}_{}.csv".format(test_name, int(time.time()), self.file_no),
"a")
self.f.write("num,acq,masked\n")
self.f.write(str(
"{},{},{}\n".format(len(self.agents)+50, self.coordinator.acquisition, self.coordinator.acq_mod)))
self.f.write("step,mse,qty,time,t_dist\n")
mse = self.coordinator.get_mse(self.environment.maps['t'].T.flatten())
self.f.write("{},{},{},{},{}\n".format(0, mse, len(self.coordinator.data[1]), 0,
sum(c.distance_travelled for c in self.agents) / len(self.agents)))
def init_maps(self):
for agent in self.agents:
[self.sensors.add(sensor) for sensor in agent.sensors]
self.environment.add_new_map(self.sensors, file=self.file_no)
def load_envs_into_agents(self):
for agent in self.agents:
agent.set_agent_env(self.environment)
def select_next_drone(self):
idx = -1
dist2next = 1000000
for i in range(len(self.agents)):
future_dist = 0
# print(self.agents[0].pose)
for k in range(len(self.agents[i].path) - 1):
future_dist += np.linalg.norm(self.agents[i].path[k][:2] - self.agents[i].path[k + 1][:2])
# print(self.agents[i].path[k][:2], self.agents[i].path[k + 1][:2])
future_dist += np.linalg.norm(self.agents[i].pose[:2] - self.agents[i].path[0][:2])
# print(self.agents[i].pose[:2], self.agents[i].path[0][:2])
# print(i, future_dist)
if future_dist < dist2next:
idx = i
dist2next = future_dist
# if np.linalg.norm(self.agents[i].pose[:2] - self.agents[i].next_pose[:2]) < dist2next:
# idx = i
# dist2next = np.linalg.norm(self.agents[i].pose[:2] - self.agents[i].next_pose[:2])
return idx, dist2next
def run_simulation(self):
imax = 24
i = 0
while i < imax:
# if not self.sender.should_update():
# plt.pause(0.5)
# continue
# t0 = time.time()
for agent in self.agents:
if agent.reached_pose():
print(agent.drone_id, "reached")
agent.next_pose = self.coordinator.generate_new_goal(pose=agent.pose, idx=i,
other_poses=[agt.pose for agt in self.agents if
agt is not agent])
self.sender.send_new_goal_msg(agent.next_pose, agent.drone_id)
else:
print(agent.drone_id, "not reached: ", agent.pose, agent.next_pose, agent.path)
next_idx, dist2_simulate = self.select_next_drone()
for agent in self.agents:
print(agent.drone_id, " is next")
if agent.step(dist_left=dist2_simulate):
if agent.reached_pose():
# time.sleep(0.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]), agent.drone_id)
self.coordinator.fit_data()
# else:
# i -= 1
self.sender.send_new_drone_msg(agent.pose, agent.drone_id)
mse = self.coordinator.get_mse(self.environment.maps['t'].T.flatten())
score = self.coordinator.get_score(self.environment.maps['t'].T.flatten())
# Theta0 = np.logspace(np.log(self.coordinator.gp.kernel_.theta / 10),
# np.log(self.coordinator.gp.kernel_.theta + 10), 500)
# LML = np.array(
# [-self.coordinator.gp.log_marginal_likelihood(np.log([Theta0[i]])) for i in range(Theta0.shape[0])])
# plt.plot(Theta0, LML)
# plt.plot(Theta0[np.where(LML == np.min(LML))], np.min(LML), '*')
# plt.title("MSE is {}".format(mse))
# plt.draw()
# plt.pause(2)
i += 1
# if i == 7:
# # if self.agents[0].distance_travelled > 1500:
# print(mse)
# with open('E:/ETSI/Proyecto/data/Databases/numpy_files/best_bo.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, len(self.coordinator.data[1]), score,
sum(c.distance_travelled for c in self.agents) / len(self.agents))) #
# print("done")
plt.show(block=True)
# import os
# os.system("pause")
self.sender.client.disconnect()
if self.saving:
self.f.close()
def __init__(self, map_path2yaml, agents: list, main_sensor, saving=False, test_name="", acq="gaussian_sei",
acq_mod="normal", file=0, initial_pos="circle"):
"""
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 sppa.SimpleAgent
Agents
Returns
-------
out : Simulator
See Also
--------
Examples
--------
"""
self.saving = saving
self.file_no = file
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
self.sender = Sender()
# if initial_pos == "none":
# print("random initial positions")
# elif initial_pos == "circle":
# print("circle initial positions")
# initial_positions = get_init_pos4(n=len(agents))
# elif initial_pos == "cvt":
# print("cvt initial positions")
initial_positions = get_init_pos4(n=len(agents), map_data=self.environment.grid, expand=True)
for agent in self.agents:
aux_f = agent.position_flag
agent.position_flag = False
if initial_pos == "circle":
agent.pose, initial_positions = initial_positions[0, :], initial_positions[1:, :]
else:
agent.randomize_pos()
agent.randomize_pos()
# agent.randomize_pos(near=False)
agent.next_pose = deepcopy(agent.pose)
agent.position_flag = aux_f
self.sender.send_new_drone_msg(agent.pose, agent.drone_id)
self.sender.send_new_goal_msg(agent.next_pose, agent.drone_id)
self.coordinator = Coordinator(self.environment.grid, self.main_sensor, acq=acq, acq_mod=acq_mod)
self.sender.send_new_acq_msg(self.coordinator.acquisition)
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)
]
[reads.append(read.read()) for read in self.agents]
for [read, my_id] in zip(reads, [0, 1, 2, 3, 4]):
self.sender.send_new_sensor_msg(str(read[0][0]) + "," + str(read[0][1]) + "," + str(read[1]),
_id=my_id)
self.sender.send_new_drone_msg(self.agents[my_id].pose, my_id)
plt.pause(0.01)
# plt.plot(reads[0][0][0], reads[0][0][1], '^y', markersize=12, label="Previous Positions")
self.coordinator.initialize_data_gpr(reads)
# from copy import copy
# import matplotlib.cm as cm
# current_cmap = copy(cm.get_cmap("inferno"))
# current_cmap.set_bad(color="#eaeaf2")
#
# plt.imshow(self.environment.render_maps()["t"], origin='lower', cmap=current_cmap) # YlGn_r
# cbar = plt.colorbar(orientation='vertical')
# cbar.ax.tick_params(labelsize=20)
# 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("Mask", fontsize=30)
# plt.xlabel("x", fontsize=20)
# plt.ylabel("y", fontsize=20)
# plt.xticks(fontsize=20)
# plt.yticks(fontsize=20)
# plt.draw()
# plt.pause(0.0001)
# plt.show(block=True)
if saving:
self.f = open(
"E:/ETSI/Proyecto/results/{}_{}_{}.csv".format(test_name, int(time.time()), self.file_no),
"a")
self.f.write("num,acq,masked\n")
self.f.write(str(
"{},{},{}\n".format(len(self.agents)+50, self.coordinator.acquisition, self.coordinator.acq_mod)))
self.f.write("step,mse,qty,time,t_dist\n")
mse = self.coordinator.get_mse(self.environment.maps['t'].T.flatten())
self.f.write("{},{},{},{},{}\n".format(0, mse, len(self.coordinator.data[1]), 0,
sum(c.distance_travelled for c in self.agents) / len(self.agents)))
def __init__(self,
map_path2yaml,
agents: list,
acq="gaussian_ei",
saving=False,
acq_fusion="coupled",
acq_mod="normal",
id_file=0,
render2gui=True,
initial_pos="circle",
name_file="",
d=1.0):
"""
:param map_path2yaml: file path to mapyaml
:param agents: list of simple agents. ALL agents have the same set of sensors
:param acq: str of the acquisition function name
:param acq_mod: str of the modification of the AF
:param id_file: int unique file id [0, 1, 2, ... , n]
:param render2gui: bool if visualization mode is online (performed through MQTT)
"""
# instancing variables
self.environment = Env(map_path2yaml=map_path2yaml)
self.noiseless_maps = False
self.agents = agents
# for agent in self.agents:
# assert isinstance(agent, Ga), "All agents should be instances of gym.SimpleAgent"
self.file_no = id_file
self.sensors = set()
self._init_maps()
self.coordinator = Coordinator(self.environment.grid,
self.sensors,
acq=acq,
acq_mod=acq_mod,
acq_fusion=acq_fusion,
d=d)
self.timestep = 0
# initializing environment
self._load_envs_into_agents()
# initialize drone positions
initial_positions = get_init_pos4(n=len(agents),
map_data=self.environment.grid,
expand=True)
for agent in self.agents:
aux_f = agent.position_flag
agent.position_flag = False
if initial_pos == "circle":
agent.pose, initial_positions = initial_positions[
0, :], initial_positions[1:, :]
else:
agent.randomize_pos()
agent.next_pose = deepcopy(agent.pose)
agent.position_flag = aux_f
# initial readings
reads = []
[reads.append(read.read()) for read in self.agents]
self.coordinator.initialize_data_gpr(reads)
self.saving = saving
if self.saving:
self.f = open(
path[-1] + "/results/MAMS/{}_{}_{}.csv".format(
name_file, int(time()), self.file_no), "a")
self.f.write(
"n_agent,n_sensors,acq_fusion,kernels,acq,acq_mod,prop\n")
self.f.write(
str("{},{},{},{},{},{},{}\n".format(
len(self.agents), len(self.sensors), acq_fusion,
len(self.coordinator.gps), self.coordinator.acquisition,
self.coordinator.acq_mod, d)))
# mses, scores, keys = self.reward()
scores, keys = self.reward()
titles = ""
for sensor in keys:
titles += f',score_{sensor}'
# titles += f',mse_{sensor},score_{sensor}'
results = ""
# for mse, score in zip(mses, scores):
# results += f",{mse},{score}"
for bic in scores:
results += f",{bic}"
# self.f.write(
# "step,qty,time,t_dist,avg_mse,avg_score{}\n".format(titles))
self.f.write("step,qty,time,t_dist,avg_score{}\n".format(titles))
# self.f.write("{},{},{},{},{},{}{}\n".format(self.timestep, len(self.coordinator.train_inputs), 0,
# sum(c.distance_travelled for c in self.agents) / len(
# self.agents), mean(mses), mean(scores), results))
self.f.write("{},{},{},{},{}{}\n".format(
self.timestep, len(self.coordinator.train_inputs), 0,
sum(c.distance_travelled
for c in self.agents) / len(self.agents), mean(scores),
results))
self.t0 = time()
self.render2gui = render2gui
if render2gui:
self.sender = Sender()
self.sender.send_new_acq_msg(self.coordinator.acquisition)
for agent in self.agents:
for sensor in agent.sensors:
self.sender.send_new_sensor_msg(
str(agent.read()["pos"][0]) + "," +
str(agent.read()["pos"][1]) + "," +
str(agent.read()[sensor]),
sensor=sensor,
_id=agent.drone_id)
self.sender.send_new_drone_msg(agent.pose, agent.drone_id)
self.sender.send_new_goal_msg(agent.next_pose, agent.drone_id)
class GymEnvironment(object):
def __init__(self,
map_path2yaml,
agents: list,
acq="gaussian_ei",
saving=False,
acq_fusion="coupled",
acq_mod="normal",
id_file=0,
render2gui=True,
initial_pos="circle",
name_file="",
d=1.0):
"""
:param map_path2yaml: file path to mapyaml
:param agents: list of simple agents. ALL agents have the same set of sensors
:param acq: str of the acquisition function name
:param acq_mod: str of the modification of the AF
:param id_file: int unique file id [0, 1, 2, ... , n]
:param render2gui: bool if visualization mode is online (performed through MQTT)
"""
# instancing variables
self.environment = Env(map_path2yaml=map_path2yaml)
self.noiseless_maps = False
self.agents = agents
# for agent in self.agents:
# assert isinstance(agent, Ga), "All agents should be instances of gym.SimpleAgent"
self.file_no = id_file
self.sensors = set()
self._init_maps()
self.coordinator = Coordinator(self.environment.grid,
self.sensors,
acq=acq,
acq_mod=acq_mod,
acq_fusion=acq_fusion,
d=d)
self.timestep = 0
# initializing environment
self._load_envs_into_agents()
# initialize drone positions
initial_positions = get_init_pos4(n=len(agents),
map_data=self.environment.grid,
expand=True)
for agent in self.agents:
aux_f = agent.position_flag
agent.position_flag = False
if initial_pos == "circle":
agent.pose, initial_positions = initial_positions[
0, :], initial_positions[1:, :]
else:
agent.randomize_pos()
agent.next_pose = deepcopy(agent.pose)
agent.position_flag = aux_f
# initial readings
reads = []
[reads.append(read.read()) for read in self.agents]
self.coordinator.initialize_data_gpr(reads)
self.saving = saving
if self.saving:
self.f = open(
path[-1] + "/results/MAMS/{}_{}_{}.csv".format(
name_file, int(time()), self.file_no), "a")
self.f.write(
"n_agent,n_sensors,acq_fusion,kernels,acq,acq_mod,prop\n")
self.f.write(
str("{},{},{},{},{},{},{}\n".format(
len(self.agents), len(self.sensors), acq_fusion,
len(self.coordinator.gps), self.coordinator.acquisition,
self.coordinator.acq_mod, d)))
# mses, scores, keys = self.reward()
scores, keys = self.reward()
titles = ""
for sensor in keys:
titles += f',score_{sensor}'
# titles += f',mse_{sensor},score_{sensor}'
results = ""
# for mse, score in zip(mses, scores):
# results += f",{mse},{score}"
for bic in scores:
results += f",{bic}"
# self.f.write(
# "step,qty,time,t_dist,avg_mse,avg_score{}\n".format(titles))
self.f.write("step,qty,time,t_dist,avg_score{}\n".format(titles))
# self.f.write("{},{},{},{},{},{}{}\n".format(self.timestep, len(self.coordinator.train_inputs), 0,
# sum(c.distance_travelled for c in self.agents) / len(
# self.agents), mean(mses), mean(scores), results))
self.f.write("{},{},{},{},{}{}\n".format(
self.timestep, len(self.coordinator.train_inputs), 0,
sum(c.distance_travelled
for c in self.agents) / len(self.agents), mean(scores),
results))
self.t0 = time()
self.render2gui = render2gui
if render2gui:
self.sender = Sender()
self.sender.send_new_acq_msg(self.coordinator.acquisition)
for agent in self.agents:
for sensor in agent.sensors:
self.sender.send_new_sensor_msg(
str(agent.read()["pos"][0]) + "," +
str(agent.read()["pos"][1]) + "," +
str(agent.read()[sensor]),
sensor=sensor,
_id=agent.drone_id)
self.sender.send_new_drone_msg(agent.pose, agent.drone_id)
self.sender.send_new_goal_msg(agent.next_pose, agent.drone_id)
def _init_maps(self):
"""
Loads the sensor names (str) into a set.
Loads ground truth maps in the environment for reading.
"""
for agent in self.agents:
[self.sensors.add(sensor) for sensor in agent.sensors]
self.environment.add_new_map(self.sensors,
file=self.file_no,
clone4noiseless=self.noiseless_maps)
def _load_envs_into_agents(self):
"""
After the environment is generated, each drone obtains access to the environment
"""
for agent in self.agents:
agent.set_agent_env(self.environment)
def render(self):
"""
:return: list of std, u foreach sensor
"""
return self.coordinator.surrogate()
def reset(self):
del self.coordinator
def _select_next_drone(self):
idx = -1
dist2next = 1000000
for i in range(len(self.agents)):
future_dist = 0
if len(self.agents[i].path) == 0:
break
for k in range(len(self.agents[i].path) - 1):
future_dist += norm(self.agents[i].path[k][:2] -
self.agents[i].path[k + 1][:2])
future_dist += norm(self.agents[i].pose[:2] -
self.agents[i].path[0][:2])
if future_dist == 0.0:
idx = -1
break
if future_dist < dist2next:
idx = i
dist2next = future_dist
return idx, dist2next
def step(self, action):
for pose, agent in zip(action, self.agents):
if len(pose) > 0:
agent.next_pose = pose
next_idx, dist2_simulate = self._select_next_drone()
if next_idx == -1:
self.timestep += 1
if self.saving:
results = ""
for i in range(len(self.sensors)):
results += ",-1"
self.f.write("{},{},{},{},{}{}\n".format(
self.timestep, len(self.coordinator.train_inputs), 0,
sum(c.distance_travelled
for c in self.agents) / len(self.agents), -1, results))
# results += ",-1,-1"
# self.f.write("{},{},{},{},{},{}{}\n".format(self.timestep, len(self.coordinator.train_inputs), 0,
# sum(c.distance_travelled for c in self.agents) / len(
# self.agents), -1, -1, results))
return -1, -1
for agent in self.agents:
if agent.step(dist_left=dist2_simulate):
if agent.reached_pose():
read = agent.read()
self.coordinator.add_data(read)
if self.render2gui:
for sensor in agent.sensors:
self.sender.send_new_sensor_msg(
str(agent.read()["pos"][0]) + "," +
str(agent.read()["pos"][1]) + "," +
str(agent.read()[sensor]),
sensor=sensor,
_id=agent.drone_id)
self.coordinator.fit_data()
if self.render2gui:
self.sender.send_new_drone_msg(agent.pose, agent.drone_id)
self.timestep += 1
# mses, scores, keys = self.reward()
scores, keys = self.reward()
# print(self.coordinator.train_inputs)
# print([agent.distance_travelled for agent in self.agents])
if self.saving:
results = ""
for score in scores:
results += f",{score}"
# results = ""
# for mse, score in zip(mses, scores):
# results += f",{mse},{score}"
# self.f.write(
# "{},{},{},{},{},{}{}\n".format(self.timestep, len(self.coordinator.train_inputs), time() - self.t0,
# sum(c.distance_travelled for c in self.agents) / len(
# self.agents), mean(mses), mean(scores), results))
self.f.write("{},{},{},{},{}{}\n".format(
self.timestep, len(self.coordinator.train_inputs),
time() - self.t0,
sum(c.distance_travelled
for c in self.agents) / len(self.agents), mean(scores),
results))
self.t0 = time()
return scores
def reward(self):
if self.noiseless_maps:
# mses = [self.coordinator.get_mse(self.environment.maps[f"noiseless_{key}"].flatten(), key) for key in
# self.coordinator.gps.keys()]
scores = [
self.coordinator.get_score(
self.environment.maps[f"noiseless_{key}"].flatten(), key)
for key in self.coordinator.gps.keys()
]
else:
# mses = [self.coordinator.get_mse(self.environment.maps[key].flatten(), key) for key in
# self.coordinator.gps.keys()]
scores = [
self.coordinator.get_score(
self.environment.maps[key].flatten(), key)
for key in self.coordinator.gps.keys()
]
# return mses, scores, self.coordinator.gps.keys()
return scores, self.coordinator.gps.keys()
def __init__(self,
map_path2yaml,
agents,
main_sensor,
saving=False,
test_name="",
acq="gaussian_sei",
acq_mod="normal",
file=0):
"""
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.file_no = file
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()
self.coordinator = Coordinator(self.environment.grid,
self.main_sensor,
acq=acq,
acq_mod=acq_mod)
self.sender.send_new_acq_msg(self.coordinator.acquisition)
self.use_cih_as_initial_points = True
if self.use_cih_as_initial_points:
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()
]
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]))
# plt.plot(reads[0][0][0], reads[0][0][1], '^y', markersize=12, label="Previous Positions")
# plt.plot(reads[1][0][0], reads[1][0][1], '^y', markersize=12)
# plt.plot(reads[2][0][0], reads[2][0][1], '^y', markersize=12)
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)
# from copy import copy
# import matplotlib.cm as cm
# current_cmap = copy(cm.get_cmap("inferno"))
# current_cmap.set_bad(color="#eaeaf2")
# plt.imshow(self.environment.render_maps()["t"], origin='lower', cmap=current_cmap) # YlGn_r
# cbar = plt.colorbar(orientation='vertical')
# cbar.ax.tick_params(labelsize=20)
# 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("Mask", fontsize=30)
# plt.xlabel("x", fontsize=20)
# plt.ylabel("y", fontsize=20)
# plt.xticks(fontsize=20)
# plt.yticks(fontsize=20)
# 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("{},{},{}\n".format(self.coordinator.k_name,
self.coordinator.acquisition,
self.coordinator.acq_mod)))
self.f.write("step,mse,t_dist,time\n")
mse = self.coordinator.get_mse(
self.environment.maps['t'].T.flatten())
self.f.write("{},{},{},0\n".format(
0, mse, self.agents[0].distance_travelled))
class Simulator(object):
def __init__(self,
map_path2yaml,
agents,
main_sensor,
saving=False,
test_name="",
acq="gaussian_sei",
acq_mod="normal",
file=0):
"""
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.file_no = file
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()
self.coordinator = Coordinator(self.environment.grid,
self.main_sensor,
acq=acq,
acq_mod=acq_mod)
self.sender.send_new_acq_msg(self.coordinator.acquisition)
self.use_cih_as_initial_points = True
if self.use_cih_as_initial_points:
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()
]
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]))
# plt.plot(reads[0][0][0], reads[0][0][1], '^y', markersize=12, label="Previous Positions")
# plt.plot(reads[1][0][0], reads[1][0][1], '^y', markersize=12)
# plt.plot(reads[2][0][0], reads[2][0][1], '^y', markersize=12)
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)
# from copy import copy
# import matplotlib.cm as cm
# current_cmap = copy(cm.get_cmap("inferno"))
# current_cmap.set_bad(color="#eaeaf2")
# plt.imshow(self.environment.render_maps()["t"], origin='lower', cmap=current_cmap) # YlGn_r
# cbar = plt.colorbar(orientation='vertical')
# cbar.ax.tick_params(labelsize=20)
# 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("Mask", fontsize=30)
# plt.xlabel("x", fontsize=20)
# plt.ylabel("y", fontsize=20)
# plt.xticks(fontsize=20)
# plt.yticks(fontsize=20)
# 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("{},{},{}\n".format(self.coordinator.k_name,
self.coordinator.acquisition,
self.coordinator.acq_mod)))
self.f.write("step,mse,t_dist,time\n")
mse = self.coordinator.get_mse(
self.environment.maps['t'].T.flatten())
self.f.write("{},{},{},0\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, file=self.file_no)
def load_envs_into_agents(self):
for agent in self.agents:
agent.set_agent_env(self.environment)
def run_simulation(self):
imax = 15
i = 0
while i < imax:
# if not self.sender.should_update():
# plt.pause(0.5)
# continue
t0 = time.time()
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, idx=i)
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(2)
i += 1
# if i == 7:
# print(mse)
# with open('E:/ETSI/Proyecto/data/Databases/numpy_files/best_bo.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,
time.time() - t0))
# if self.agents[0].distance_travelled > 1500:
# break
print("done")
# plt.show(block=True)
self.sender.client.disconnect()
if self.saving:
self.f.close()
def __init__(self,
map_path2yaml,
agents: list,
acq="GALM",
saving=False,
acq_fusion="GALM",
acq_mod="truncated",
id_file=0,
render2gui=True,
name_file="",
d=1.0):
"""
:param map_path2yaml: file path to mapyaml
:param agents: list of simple agents. ALL agents have the same set of sensors
:param acq: str of the acquisition function name
:param acq_mod: str of the modification of the AF
:param id_file: int unique file id 0, 1, 2, ... , n
:param render2gui: bool if visualization mode is online (performed through MQTT)
"""
# instancing variables
self.environment = Env(map_path2yaml=map_path2yaml)
self.noiseless_maps = False
self.agents = agents
self.file_no = id_file
self.sensors = set()
self._init_maps()
if acq == "ga":
self.coordinator = GACoordinator(self.environment.grid,
self.sensors,
d=d,
no_drones=len(agents))
elif acq == "lm":
self.coordinator = LMCoordinator(self.environment.grid,
self.sensors,
d=d,
no_drones=len(agents),
acq=id_file)
self.timestep = 0
self._load_envs_into_agents()
for agent in enumerate(self.agents):
aux_f = agent[1].position_flag
agent[1].position_flag = False
agent[1].pose = self.coordinator.current_goals[agent[0]]
agent[1].next_pose = deepcopy(agent[1].pose)
agent[1].position_flag = aux_f
# initial readings
# import matplotlib.pyplot as plt
# for sx, sy, cg in zip(self.coordinator.fpx, self.coordinator.fpy, self.coordinator.current_goals):
# plt.plot(sx, sy, '-x')
# plt.plot([cg[0], sx[0]], [cg[1], sy[0]], '-x')
# for a in agents:
# plt.plot(a.pose[0], a.pose[1], '*')
# plt.show(block=True)
reads = []
[reads.append(read.read()) for read in self.agents]
self.coordinator.initialize_data_gpr(reads)
self.saving = saving
if self.saving:
self.f = open(
path[-1] + "/results/MAMS/{}_{}_{}.csv".format(
name_file, int(time()), self.file_no), "a")
self.f.write(
"n_agent,n_sensors,acq_fusion,kernels,acq,acq_mod,prop\n")
self.f.write(
str("{},{},{},{},{},{},{}\n".format(len(self.agents),
len(self.sensors),
acq_fusion,
len(self.coordinator.gps),
acq, acq_mod, d)))
# mses, scores, keys = self.reward()
scores, keys = self.reward()
titles = ""
for sensor in keys:
titles += f',score_{sensor}'
# titles += f',mse_{sensor},score_{sensor}'
results = ""
# for mse, score in zip(mses, scores):
# results += f",{mse},{score}"
for bic in scores:
results += f",{bic}"
# self.f.write(
# "step,qty,time,t_dist,avg_mse,avg_score{}\n".format(titles))
self.f.write("step,qty,time,t_dist,avg_score{}\n".format(titles))
# self.f.write("{},{},{},{},{},{}{}\n".format(self.timestep, len(self.coordinator.train_inputs), 0,
# sum(c.distance_travelled for c in self.agents) / len(
# self.agents), mean(mses), mean(scores), results))
self.f.write("{},{},{},{},{}{}\n".format(
self.timestep, len(self.coordinator.train_inputs), 0,
sum(c.distance_travelled
for c in self.agents) / len(self.agents), mean(scores),
results))
self.t0 = time()
self.render2gui = render2gui
if render2gui:
self.sender = Sender()
self.sender.send_new_acq_msg(self.coordinator.acquisition)
for agent in self.agents:
for sensor in agent.sensors:
self.sender.send_new_sensor_msg(
str(agent.read()["pos"][0]) + "," +
str(agent.read()["pos"][1]) + "," +
str(agent.read()[sensor]),
sensor=sensor,
_id=agent.drone_id)
self.sender.send_new_drone_msg(agent.pose, agent.drone_id)
self.sender.send_new_goal_msg(agent.next_pose, agent.drone_id)