import field
import agent
import configs
import basic_functions as bf
import print_colors
import woa
import robotic_active_olfaction as rao
from configs import *
import time
if __name__ == "__main__":
agents_no = 3
t = 1
c_field = field.load_field(t)
plume_finding_threshold = 0.2
woa_threshold = 0.6
state = 0
finding_end = COUNTER_MAX
serial_no = str(time.strftime("%Y%m%d-%H%M%S", time.localtime()))
# agents, leader = init_agents_random(agents_no, c_field)
init_position = [-1, -1, 0.5]
agents, leader = agent.init_agents_fixed(agents_no, c_field, init_position)
bf.save_trajectory(agents, leader, serial_no)
bf.save_results(agents, leader, serial_no, finding_end)
bf.show_info(agents, leader, t, state)
while leader.concentration <= plume_finding_threshold and len(leader.history) < COUNTER_MAX:
# @Author : Luc # @Email : [email protected] # @File : draw_plane.py from mpl_toolkits.mplot3d import axes3d import matplotlib.pyplot as plt import field import configs import numpy as np import matplotlib.cm as cm time = 180 accuracy = 0.05 height = 1.3 nodes = field.load_field(time) plane = [] for node in nodes: if node[2] == 0.5: plane.append(node) plane = np.array(plane) X = np.arange(-2.2, 2.2, 0.05) Y = np.arange(-2.6, 2.8, 0.05) X_GRID, Y_GRID = np.meshgrid(X, Y) C_GRID = np.empty((len(X), len(Y))) for i in range(len(X)): for j in range(len(Y)): xx = float(round(X[i], 2)) yy = float(round(Y[j], 2))
def source_localization_2d(agents_no, finding_threshold, tracing_threshold,
height):
SUCCESS = False
state = 0
t = 1
c_field = field.load_field(t)
finding_end = COUNTER_MAX
serial_no = str(time.strftime("%Y%m%d-%H%M%S", time.localtime()))
agents, leader = agent.init_agents_fixed(agents_no, c_field,
[-2, -2, height])
# agents, leader = init_agents_random(agents_no, c_field)
while len(leader.history) < COUNTER_MAX and (not SUCCESS):
if state == 0:
while len(leader.history) < COUNTER_MAX:
t = len(leader.history) * 2
if t >= 200:
t = 200
c_field = field.load_field(t)
agents, leader = rao.plume_finding_2d(agents, leader, c_field,
height)
# bf.show_info(agents, leader, t, state)
if leader.concentration > finding_threshold:
state = 1
break
finding_end = len(leader.history)
elif state == 1:
while len(leader.history) < COUNTER_MAX:
t = len(leader.history) * 2
if t >= 200:
t = 200
c_field = field.load_field(t)
agents, leader = rao.plume_tracking_2d(agents, leader, c_field,
height)
bf.show_info(agents, leader, t, state)
if leader.concentration > tracing_threshold:
SUCCESS = True
break
if agent.get_leader_age(leader) > 5:
local_maximum = leader.concentration
state = 2
break
elif state == 2:
while len(leader.history) < COUNTER_MAX:
t = len(leader.history) * 2
if t >= 200:
t = 200
c_field = field.load_field(t)
agents, leader = rao.plume_finding_2d(agents, leader, c_field,
height)
bf.show_info(agents, leader, t, state)
if leader.concentration > tracing_threshold:
SUCCESS = True
break
if leader.concentration > local_maximum + (tracing_threshold *
0.1):
state = 1
break
tracing_end = len(leader.history)
if np.linalg.norm(leader.history[-1][0] -
np.array([0, -0.3, height])) < 0.3:
SUCCESS = True
else:
SUCCESS = False
bf.save_trajectory(agents, leader, serial_no)
bf.save_results(agents, leader, serial_no, finding_end)
return finding_end, tracing_end, SUCCESS