import numpy as np
import matplotlib.pyplot as plt
from gridworld import Grid
import gridworld
from AcousticTag import AcousticTag
from AcousticReciever import AcousticReciever
taglist = []
E = Grid(taglist, x_range=100, y_range=100)
taglist = E.loadTagList(fname="testField") #load as list of tags
tagData = E.nploadTagList(fname="testField") #load tagData into numpy array
############################################## Plotting ############################
def draw(x, fig=1): #function for plotting field
plt.figure(fig)
plt.axis('scaled')
plt.grid(True)
plt.plot(x[0], x[1], 'r.')
plt.xlim([0, E.x_range])
plt.ylim([0, E.y_range])
plt.xticks(np.arange(0, E.x_range, E.x_range / 5.0))
plt.yticks(np.arange(0, E.y_range, E.y_range / 5.0))
plt.draw()
#get all tag position to plot
N = len(taglist)
tagx = np.zeros(N)
tagy = np.zeros(N)
taglist = []
agentList = []
tagx = np.zeros(N)
tagy = np.zeros(N)
for i in range(N):
# taglist.append(AcousticTag(i,last_ping=np.random.randn()),ping_delay=max(2,30*np.random.randn())) # most realistic
taglist.append(AcousticTag(
i, last_ping=17 *
np.random.randn())) # more realistic (pings are not aligned in time)
# taglist.append(AcousticTag(i)) #better for understanding because pings are aligned in time and all have same ping interval
x, y, _ = taglist[i].pos
tagx[i] = x
tagy[i] = y
E = Grid(taglist, x_range=x_range, y_range=y_range)
if field == fields[0]:
taglist = E.loadTagList() # E.setMap(density_map)
# E.saveTagList()
for i in range(numAgents):
s = AcousticReciever(np.array([0, 0, 0]), sensorRange)
if method == searchMethods[2]:
# agentList.append(Agent(np.array([np.random.rand()*x_range,np.random.rand()*y_range,0,0]),s,E,dim=2))
agentList.append(
Agent(np.array([start_pos[0], start_pos[1], 0, 0]), s, E, dim=2))
agentList[i].dynamics = m2_step
u = [0, 0]
else:
# agentList.append(Agent(np.array([np.random.rand()*x_range,np.random.rand()*y_range]),s,E,dim=2))
agentList.append(
Agent(np.array([start_pos[0], start_pos[1]]), s, E, dim=2))
plt.xticks(np.arange(0, grid.x_range, grid.x_range / 5.0))
plt.yticks(np.arange(0, grid.y_range, grid.y_range / 5.0))
plt.draw()
taglist = []
N = 100 # number of tags to be generated in field
#generate N tags and append to taglist
for i in range(N):
delay = max(1, 30 * np.random.rand())
#each tag object has an ID, an offset (last_ping), and a delay between pings (ping_delay)
taglist.append(
AcousticTag(i, last_ping=-delay * np.random.rand(), ping_delay=delay))
# create grid world object
grid = Grid(taglist, x_range=100, y_range=100)
#make map of densities for each cell
density_map = np.array([
0.1, 0.1, 0.4, 0.3, 0.2, 0.1, 0.3, 0.3, 0.1, 0.3, 0.2, 0.3, 0.3, 0.2, 0.1,
0.3, 0.9, 0.3, 0.2, 0.1, 0.2, 0.3, 0.2, 0.1, 0.1
])
grid.setMap(density_map) #set map with no arguement sets the default map
#saveTaglist to .csv file
grid.saveTagList(fname="testField")
############################################### Plotting ############################
#get all tag position to plot
tagx = np.zeros(N)
tagy = np.zeros(N)
for i in range(len(taglist)):
x, y, _ = taglist[i].pos
taglist = []
agentList = []
tagx = np.zeros(N)
tagy = np.zeros(N)
for i in range(N):
#taglist.append(AcousticTag(i,last_ping=30*np.random.randn()),ping_delay=max(2,30*np.random.randn())) # most realistic
taglist.append(AcousticTag(
i, last_ping=15 *
np.random.randn())) # more realistic (pings are not aligned in time)
#taglist.append(AcousticTag(i)) #better for understanding because pings are aligned in time and all have same ping interval
x, y, _ = taglist[i].pos
tagx[i] = x
tagy[i] = y
E = Grid(taglist)
E.setMap(density_map)
for i in range(numAgents):
s = AcousticReciever(np.array([0, 0, 0]), 50)
agentList.append(Agent(np.array([(i * 2 + 1) * 50.0, 605.0]), s, E, dim=2))
agentList[i].dynamics = m1_step
for i in range(N):
x, y, _ = taglist[i].pos
tagx[i] = x
tagy[i] = y
#draw((tagx,tagy))
'''
for t in range(N):
from Agent import Agent
from AcousticReciever import AcousticReciever
from gridworld import Grid
import numpy as np
def m2_step(x, u): #motion model
# |0 0 1 0|x |0 0|
# |0 0 0 1|y + |0 0|u1
# |0 0 -a 0|vx |1 0|u2
# |0 0 0 -b|vy |0 1|
a = .25
return np.matmul(
1 *
np.array([[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, -a, 0], [0, 0, 0, -a]]),
x) + np.matmul(1 * np.array([[0, 0], [0, 0], [1, 0], [0, 1]]), u)
#an agent needs a sensor object
#the sensor needs a position and range
sensorRange = 5
sensor = AcousticReciever(np.array([0, 0, 0]), sensorRange)
#it also needs a gridworld object for access to the tag locations and possibly flow model. may get rid of the equirment in future
taglist = []
E = Grid(taglist, x_range=100, y_range=100)
#agent takes initial state array, a sensor, and the number of dimensions (2 for (x,y) vs 3 for (x,y,z)) can also be added
rd2d = Agent(np.array([1, 1, 0, 0]), sensor, E, dim=2)
rd2d.dynamics = m2_step #then agent takes a function as the dynamics with the input signature f(x,u)
running=False searchComplete=False taglist=[] agentList=[] tagx=np.zeros(N) tagy=np.zeros(N) for i in range(N): #taglist.append(AcousticTag(i,last_ping=np.random.randn()),ping_delay=max(2,30*np.random.randn())) # most realistic taglist.append(AcousticTag(i,last_ping=np.random.randn())) # more realistic (pings are not aligned in time) #taglist.append(AcousticTag(i)) #better for understanding because pings are aligned in time and all have same ping interval x,y,_ = taglist[i].pos tagx[i]=x tagy[i]=y E = Grid(taglist) E.setMap(density_map) for i in range(numAgents): s= AcousticReciever(np.array([0,0,0]),50) agentList.append(Agent(np.array([(i*2+1)*50.0,605.0]),s,E,dim=2)) agentList[i].dynamics=m1_step for i in range(N): x,y,_ = taglist[i].pos tagx[i]=x tagy[i]=y #draw((tagx,tagy)) '''
show_only_when_pinging = True
#setup
taglist=[]
agentList=[]
tagx=np.zeros(N)
tagy=np.zeros(N)
for i in range(N):
#taglist.append(AcousticTag(i,last_ping=np.random.randn()),ping_delay=max(2,30*np.random.randn())) # most realistic
taglist.append(AcousticTag(i,last_ping=10*np.random.randn())) # more realistic (pings are not aligned in time)
#taglist.append(AcousticTag(i)) #better for understanding because pings are aligned in time and all have same ping interval
x,y,_ = taglist[i].pos
tagx[i]=x
tagy[i]=y
#E = Grid(taglist,x_range=20,y_range=20)
E = Grid(taglist)
if fileToLoad=="":
E.setMap()
#E.saveTagList()
else:
taglist=E.loadTagList()
for i in range(numAgents):
s= AcousticReciever(np.array([0,0,0]),50)
agentList.append(Agent(np.array([(i+1)*E.x_range/5,3*E.y_range/5]),s,E,dim=2))
agentList[i].dynamics=m1_step
for i in range(N):
x,y,_ = taglist[i].pos
tagx[i]=x
tagy[i]=y
numAgents=1
time_step=.5
#np.random.seed(1)
#setup
taglist=[]
agentList=[]
tagx=np.zeros(N)
tagy=np.zeros(N)
for i in range(N):
taglist.append(AcousticTag(i,last_ping=15*np.random.randn())) # more realistic
#taglist.append(AcousticTag(i)) #better for understanding
x,y,_ = taglist[i].pos
tagx[i]=x
tagy[i]=y
E = Grid(taglist)
E.setMap()
for i in range(numAgents):
s= AcousticReciever(np.array([0,0,0]),50)
agentList.append(Agent(np.array([50.0,600.0]),s,E,dim=2))
agentList[i].dynamics=m1_step
for i in range(N):
x,y,_ = taglist[i].pos
tagx[i]=x
tagy[i]=y
draw((tagx,tagy))
'''
running = False
searchComplete = False
taglist = []
agentList = []
tagx = np.zeros(N)
tagy = np.zeros(N)
for i in range(N):
#taglist.append(AcousticTag(i,last_ping=np.random.randn()),ping_delay=max(2,30*np.random.randn())) # most realistic
taglist.append(AcousticTag(i, last_ping=np.random.randn())
) # more realistic (pings are not aligned in time)
#taglist.append(AcousticTag(i)) #better for understanding because pings are aligned in time and all have same ping interval
x, y, _ = taglist[i].pos
tagx[i] = x
tagy[i] = y
E = Grid(taglist)
E.setMap(density_map)
for i in range(numAgents):
s = AcousticReciever(np.array([0, 0, 0]), 50)
agentList.append(Agent(np.array([(i * 2 + 1) * 50.0, 605.0]), s, E, dim=2))
agentList[i].dynamics = m1_step
for i in range(N):
x, y, _ = taglist[i].pos
tagx[i] = x
tagy[i] = y
#draw((tagx,tagy))
'''
for t in range(N):
taglist=[]
agentList=[]
tagx=np.zeros(N)
tagy=np.zeros(N)
"""
for i in range(N):
#taglist.append(AcousticTag(i,last_ping=-np.random.rand()),ping_delay=max(2,30*np.random.randn())) # most realistic
taglist.append(AcousticTag(i,last_ping=-17*np.random.rand())) # more realistic (pings are not aligned in time)
# taglist.append(AcousticTag(i)) #better for understanding because pings are aligned in time and all have same ping interval
x,y,_ = taglist[i].pos
tagx[i]=x
tagy[i]=y
"""
E = Grid(taglist,x_range=x_range, y_range=y_range)
if field == fields[0]:
taglist= E.loadTagList("testField1_1000") #E.setMap(density_map)
tagData=np.genfromtxt("testField1_1000.csv",delimiter=",")
#E.saveTagList("tags")
for i in range(numAgents):
s= AcousticReciever(np.array([0,0,0]),sensorRange)
if method == searchMethods[2]:
#agentList.append(Agent(np.array([np.random.rand()*x_range,np.random.rand()*y_range,0,0]),s,E,dim=2))
agentList.append(Agent(np.array([start_pos[0],start_pos[1],0,0]),s,E,dim=2))
agentList[i].dynamics=m2_step
u=[0,0]
elif method == searchMethods[4]:
#agentList.append(Agent(np.array([np.random.rand()*x_range,np.random.rand()*y_range,0,0]),s,E,dim=2))
agentList.append(Agent(np.array([start_pos[0],start_pos[1]]),s,E,dim=2))
agentList[i].dynamics=m3_step