def __init__(self, sf, init_weight, sensorflyOwnMap, sf0):
'''
Constructor
'''
self.weight = init_weight
self.landmarkDict = {}
# Local coverage map for each particle
self.localMap = GridMap(sensorflyOwnMap.length,
sensorflyOwnMap.breadth,
sensorflyOwnMap.celllength,
sensorflyOwnMap.cellbreadth)
if 1:
self.pos = self.localMap.xytocellNoBoundaryCheck(
sf.x - sf0.x, sf.y - sf0.y)
self.pos = [
self.pos[0] + int(self.localMap.gridlength / 2),
self.pos[1] + int(self.localMap.gridbreadth / 2)
]
self.lastpos = self.localMap.xytocellNoBoundaryCheck(
sf.x - sf0.x, sf.y - sf0.y)
self.lastpos = [
self.lastpos[0] + int(self.localMap.gridlength / 2),
self.lastpos[1] + int(self.localMap.gridbreadth / 2)
]
else:
self.pos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
self.lastpos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
# SensorFly constructor
SensorFly.__init__(self, sf.name, sf.x, sf.y, sf.dir, sf.battery, sf.noiseVelocity, sf.noise_turn, \
sf.noiseRadio)
def __init__(self, _init_coverageProbMap, num_particles, sflist):
'''
Constructor
'''
# Initialize attributes
self.c_map = _init_coverageProbMap
self.num_particles = num_particles
self.numRevisit = 0
self.numlandmarks = 0
self.control = SMController()
self.localMaps = []
self.landmarks = {}
# Initialize particle dictionary
self.particles = {}
sf0 = sflist[0] # the location of the 1st sensorfly in the real world is used as a relative origin
for sf in sflist:
# Keep a local coverage map for each SensorFly
sensorflyOwnMap = GridMap(self.c_map.length, self.c_map.breadth,
self.c_map.celllength, self.c_map.cellbreadth)
self.localMaps.append(sensorflyOwnMap)
plist = []
pwts = []
for i in range(num_particles): #@UnusedVariable
# A list of particles for each SensorFly
p = Particle(sf, float(1)/float(num_particles), sensorflyOwnMap, sf0)
plist.append(p)
pwts.append(float(1)/float(num_particles))
self.particles[sf.name] = [plist, pwts]
def __init__(self, sf, init_weight, sensorflyOwnMap, sf0):
'''
Constructor
'''
self.weight = init_weight
self.landmarkDict = {}
# Local coverage map for each particle
self.localMap = GridMap(sensorflyOwnMap.length, sensorflyOwnMap.breadth,
sensorflyOwnMap.celllength, sensorflyOwnMap.cellbreadth)
if 1:
self.pos = self.localMap.xytocellNoBoundaryCheck(sf.x-sf0.x, sf.y-sf0.y)
self.pos = [self.pos[0] + int(self.localMap.gridlength/2), self.pos[1] + int(self.localMap.gridbreadth/2)]
self.lastpos = self.localMap.xytocellNoBoundaryCheck(sf.x-sf0.x, sf.y-sf0.y)
self.lastpos = [self.lastpos[0] + int(self.localMap.gridlength/2), self.lastpos[1] + int(self.localMap.gridbreadth/2)]
else:
self.pos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
self.lastpos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
# SensorFly constructor
SensorFly.__init__(self, sf.name, sf.x, sf.y, sf.dir, sf.battery, sf.noiseVelocity, sf.noise_turn, \
sf.noiseRadio)
class Controller(object):
'''
Simulation Engine
'''
def __init__(self, arena):
'''
Constructor
Params: arena - Arena object for the simulation
'''
self.arena = arena
self.explorers = list()
self.anchors = list()
self.base = [1, 1]
self.deltick = []
#added by xinlei
self.state = "Unregistered"
self.round = 0
def addExplorer(self, sf):
'''
Add a SensorFly to the simulation
'''
self.explorers.append(sf)
def addExplorers(self, explorers):
'''
Add list of SensorFly objects in the simulation
'''
for sf in explorers:
self.explorers.append(sf)
def removeExplorer(self, name):
'''
Remove a SensorFly from the simulation
'''
self.explorers = filter(lambda sf: sf.name != name, self.explorers)
def setExplorerStartLocation(self, base):
# this function sets start locations of explorers around the given
# "base" and "radius"
self.base = base
sfnum = len(self.explorers)
radius = sfnum * 2
# get x range
if base[0] < radius / 2 + 2:
x_range = range(1, radius)
elif base[0] > self.arena.gridmap.gridlength - 2 - radius / 2:
x_range = range(self.arena.gridmap.gridlength -
radius - 1, self.arena.gridmap.gridlength - 2)
else:
x_range = range(base[0] - radius / 2, base[0] + radius / 2 - 1)
# get y range
if base[1] < radius / 2 + 2:
yrange = range(1, radius)
elif base[1] > self.arena.gridmap.gridbreadth - 2 - radius / 2:
yrange = range(self.arena.gridmap.gridbreadth -
radius - 1, self.arena.gridmap.gridbreadth - 2)
else:
yrange = range(base[1] - radius / 2, base[1] + radius / 2 - 1)
for sf in self.explorers:
i = 0
while i < 100:
x = random.sample(set(x_range), 1)[0]
y = random.sample(set(yrange), 1)[0]
if self.arena.gridmap.map[x][y] != self.arena.gridmap.v_obstacle:
sf.x = x
sf.y = y
break
i += 1
if i >= 100:
return False
return True
def addAnchor(self, sf):
'''
Add a SensorFly to the simulation
'''
self.anchors.append(sf)
def addAnchors(self, anchors):
'''
Add list of SensorFly objects in the simulation
'''
for sf in anchors:
self.anchors.append(sf)
def removeAnchor(self, name):
'''
Remove a SensorFly from the simulation
'''
self.anchors = filter(lambda sf: sf.name != name, self.anchors)
def getExplorers(self):
return self.explorers
def getAnchors(self):
return self.anchors
# added by xinlei
def _state_cnt(self,algo,record,ser):
for sf in self.explorers:
#print sf.state,"explorer no.", sf.id
#print clients[sf.id]
#print data[sf.id]
if sf.is_goal_reached:
sf.state = "Moved"
if sf.state == "Unregistered":
#for print
if sf.p_flag_1:
print "explorer",sf.id,',',sf.state
sf.p_flag_1 = False
sf.p_flag_2 = True
elif sf.state == "Registered":
#for print
if sf.p_flag_2:
print "explorer",sf.id,',',sf.state
sf.p_flag_2 = False
sf.p_flag_1 = True
sf.rssi_rcving_flag = True
sf.gnd_trth_rcd_flag = False
sf.real_updated = False
sf.pf_updated_flag = False
sf.cmd_set_flag = False
elif sf.state == "GroundTruth":
if ser.isOpen()==False:
ser.open()
xbee = XBee(ser)
#for print
if sf.p_flag_1:
self.round += 1
print "explorer",sf.id,',',sf.state
sf.p_flag_1 = False
sf.p_flag_2 = True
#receive sf.xy
#print self.state
#sf.is_moving = False
self.updateReal(sf)
if sf.rssi_rcving_flag:
sf.reset_rssi()
sf.get_rssi(xbee)
#print "getting rssi for explorer no.", sf.id
#if sf.rssi_rcv_all and sf.cmd_set_flag == False:
algo.update(clients,sf) #needed to be changed
algo.command(clients,sf)
sf.file_rcd_flag = False
#print "explorer no.", sf.id,"[", sf.command.velocity,sf.command.turn, "}"
elif sf.state == "Directions":
#for print
if sf.p_flag_2:
print "explorer",sf.id,',',sf.state
sf.p_flag_2 = False
sf.p_flag_1 = True
if not sf.file_rcd_flag:
record_str =str(self.round)+ ',' + sf.name+','+str(sf.xy[0])+','+ str(sf.xy[1])+','+ \
str(sf.pf_estimated_xy[0])+','+str(sf.pf_estimated_xy[1])+','+ \
str(sf.dr_estimated_xy[0])+','+str(sf.dr_estimated_xy[1])+','+str(pct_covered)+','+\
str(sig_match_cnt)+','+str(sf.certainty)+','+str(sf.rssi[0])+','+str(sf.rssi[1])+','+\
str(sf.rssi[2])+','+str(sf.rssi[3])+','+str(sf.rssi[4])+','+str(sf.rssi[5])+ ','+\
str(sf.mag_dir)+','+str(sf.opt)+','+str(int(sf.is_goal_reached))+ ','+\
str(sf.command.turn)+ ','+ str(sf.command.velocity*sf.command.time)+','+\
str(sf.sig_xy[0])+','+str(sf.sig_xy[1])+'\n'
print record_str
#filestr1 = './'+filestr
with open(filestr,'a') as myfile:
myfile.write(record_str)
print "Recording"
sf.file_rcd_flag = True
sf.mag_rcving_flag = True
sf.mag_rcd_flag = True
elif sf.state == "Rotating":
#for print
if sf.p_flag_1:
print "explorer",sf.id,',',sf.state
sf.p_flag_1 = False
sf.p_flag_2 = True
#start Rotating
# print self.state
#sf.is_moving = True
sf.get_opt_mag(xbee, data)
sf.mag_rcving_flag = True
sf.mag_rcd_flag = True
print data[sf.id]
elif sf.state == "Rotated":
#for print
if sf.p_flag_2:
print "explorer",sf.id,',',sf.state
sf.p_flag_2 = False
sf.p_flag_1 = True
#stop rotating
#print self.state
#sf.is_moving = False
sf.get_opt_mag(xbee,data)
if sf.mag_rcd_flag:
print sf.mag_dir
sf.mag_rcving_flag = False
sf.mag_rcd_flag = False
sf.rst_opt_flag = True
#print sf.mag_dir
#get sf.mag_dir and update
elif sf.state == "Moving":
#for print
if sf.p_flag_1:
print "explorer",sf.id,',',sf.state
sf.p_flag_1 = False
sf.p_flag_2 = True
#start moving
#print self.state
#sf.is_moving = True
sf.send_rst_opt(xbee) #reset px4flow distance calculation
sf.get_opt_mag(xbee, data)
sf.opt_rcving_flag = True
sf.opt_rcd_flag = True
print data[sf.id]
elif sf.state == "Moved":
#for print
if sf.p_flag_2:
print "explorer",sf.id,',',sf.state
sf.p_flag_2 = False
sf.p_flag_1 = True
sf.gnd_trth_rcd_flag = False
sf.rssi_rcving_flag = True
sf.real_updated = False
sf.cmd_set_flag = False
sf.pf_updated_flag = False
#sf.is_moving = False
#sf.has_turned = False
sf.get_opt_mag(xbee,data)
if sf.opt_rcd_flag:
print sf.opt
sf.opt_rcving_flag =False
sf.opt_rcd_flag = False
if ser.isopen():
ser.close()
#print sf.opt
#get sf.mag_dir and update
def run(self, num_ticks, case, pbar, it,inputClients, inputData,systemRunning,inputfile_str):
'''
Run the simulation
Params: runtime - time to run in seconds
deltick - one tick in seconds
'''
global clients
global data
global filestr
clients = inputClients
data = inputData
filestr = inputfile_str
#added by xinlei for human experiment
ser = serial.Serial('/dev/tty.usbserial-A5025X6Z',38400)
#infostr = case.name +','+ str(case.num_explorers) +','+ str(case.num_anchors), case.num_particles, case.max_iterations, \
# case.noise_radio, case.noise_velocity, case.noise_turn, case.noise_mag]
self.c_map = GridMap(np.zeros(self.arena.gridmap.map.shape))
self.deltick = case.deltick
# Change the algorithm here
algo = dw.DrunkWalk(self.explorers, self.c_map, case)
# Display
if case.is_display_on_real:
self.arena.gridmap.displayInit()
record = []
# Main loop
state_counter =3
#for tick in range(0, num_ticks):
while systemRunning[0]:
# self.state = clients[0]["state"]
#pbar.update((num_ticks * it) + tick)
#algo.command() #get command and set command for each explorer
#self.updateReal() #execute command and update SensorFly location in real arena
#algo.update() #Update the estimates as per command
'''
if (tick%100==0):
if (state_counter<7):
state_counter +=1
else:
state_counter = 4
self.state = States[state_counter]
self.state = States[4]
'''
#update state
for sf in self.explorers:
sf.state = clients[sf.id]["state"]
#sf.state = self.state
self._state_cnt(algo,record,ser)
#sleep(.05)
global pct_covered
global is_all_covered
# Record data
if case.goal_graph:
[pct_covered, is_all_covered] = case.goal_graph.getCoverage()
else:
pct_covered = 0
# goal_reached = [sf.is_goal_reached for sf in self.explorers]
global sig_match_cnt
sig_match_cnt = sum([sf.sig_match_cnt for sf in self.explorers])
'''
for sf in self.explorers:
record.append([1, int(sf.name), sf.xy[0], sf.xy[1], \
sf.pf_estimated_xy[0], sf.pf_estimated_xy[1], \
#sf.dr_estimated_xy[0], sf.dr_estimated_xy[1], pct_covered, sig_match_cnt])
sf.dr_estimated_xy[0], sf.dr_estimated_xy[1], pct_covered, sig_match_cnt,sf.certainty,sf.rssi]) #modified by xinlei
'''
# Visualize
if case.is_display_on_real:
self.arena.gridmap.displayUpdate()
if case.stop_on_all_covered and case.goal_graph and is_all_covered:
break
class Particle(SensorFly):
'''
Particle for sensorfly
'''
def __init__(self, sf, init_weight, sensorflyOwnMap, sf0):
'''
Constructor
'''
self.weight = init_weight
self.landmarkDict = {}
# Local coverage map for each particle
self.localMap = GridMap(sensorflyOwnMap.length,
sensorflyOwnMap.breadth,
sensorflyOwnMap.celllength,
sensorflyOwnMap.cellbreadth)
if 1:
self.pos = self.localMap.xytocellNoBoundaryCheck(
sf.x - sf0.x, sf.y - sf0.y)
self.pos = [
self.pos[0] + int(self.localMap.gridlength / 2),
self.pos[1] + int(self.localMap.gridbreadth / 2)
]
self.lastpos = self.localMap.xytocellNoBoundaryCheck(
sf.x - sf0.x, sf.y - sf0.y)
self.lastpos = [
self.lastpos[0] + int(self.localMap.gridlength / 2),
self.lastpos[1] + int(self.localMap.gridbreadth / 2)
]
else:
self.pos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
self.lastpos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
# SensorFly constructor
SensorFly.__init__(self, sf.name, sf.x, sf.y, sf.dir, sf.battery, sf.noiseVelocity, sf.noise_turn, \
sf.noiseRadio)
def setMoveCommand(self, turn, velocity):
'''
Params: turn - the angle to turn in degrees
time - the time to setMoveCommand in seconds
'''
# Set the command to be executed now
self.nowCommand.turn = self.odometer.turn(turn)
self.nowCommand.velocity = self.odometer.velocity(velocity)
self.dir = (self.dir + self.nowCommand.turn) % 360
def update(self, sf, deltick):
'''
Params: deltick - the time tick in seconds
'''
# Move
if sf.hasCollided == False:
oldposxy = [self.pos[0], self.pos[1]]
self.pos[0] = self.pos[0] + (self.nowCommand.velocity * deltick
) * math.cos(math.radians(self.dir))
self.pos[1] = self.pos[1] + (self.nowCommand.velocity * deltick
) * math.sin(math.radians(self.dir))
newposxy = self.pos
self.updateLocalMap(oldposxy, newposxy)
# Check if SensorFly has collided
# TODO: Fix the collision coordinates
self.hasCollided = sf.hasCollided
def updateLocalMap(self, oldposxy, newposxy):
'''
Update covered local map
'''
# Mark new position as covered
# TODO: Add coverage to all cells along path
opos = self.localMap.xytocell(oldposxy[0], oldposxy[1])
npos = self.localMap.xytocell(newposxy[0], newposxy[1])
self.pos = npos
self.lastpos = opos
self.localMap.markMap(npos[0], npos[1], 1)
class Particle(SensorFly):
'''
Particle for sensorfly
'''
def __init__(self, sf, init_weight, sensorflyOwnMap, sf0):
'''
Constructor
'''
self.weight = init_weight
self.landmarkDict = {}
# Local coverage map for each particle
self.localMap = GridMap(sensorflyOwnMap.length, sensorflyOwnMap.breadth,
sensorflyOwnMap.celllength, sensorflyOwnMap.cellbreadth)
if 1:
self.pos = self.localMap.xytocellNoBoundaryCheck(sf.x-sf0.x, sf.y-sf0.y)
self.pos = [self.pos[0] + int(self.localMap.gridlength/2), self.pos[1] + int(self.localMap.gridbreadth/2)]
self.lastpos = self.localMap.xytocellNoBoundaryCheck(sf.x-sf0.x, sf.y-sf0.y)
self.lastpos = [self.lastpos[0] + int(self.localMap.gridlength/2), self.lastpos[1] + int(self.localMap.gridbreadth/2)]
else:
self.pos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
self.lastpos = self.localMap.xytocellNoBoundaryCheck(sf.x, sf.y)
# SensorFly constructor
SensorFly.__init__(self, sf.name, sf.x, sf.y, sf.dir, sf.battery, sf.noiseVelocity, sf.noise_turn, \
sf.noiseRadio)
def setMoveCommand(self, turn, velocity):
'''
Params: turn - the angle to turn in degrees
time - the time to setMoveCommand in seconds
'''
# Set the command to be executed now
self.nowCommand.turn = self.odometer.turn(turn)
self.nowCommand.velocity = self.odometer.velocity(velocity)
self.dir = (self.dir + self.nowCommand.turn) % 360
def update(self, sf, deltick):
'''
Params: deltick - the time tick in seconds
'''
# Move
if sf.hasCollided == False:
oldposxy = [self.pos[0], self.pos[1]]
self.pos[0] = self.pos[0] + (self.nowCommand.velocity * deltick) * math.cos(math.radians(self.dir))
self.pos[1] = self.pos[1] + (self.nowCommand.velocity * deltick) * math.sin(math.radians(self.dir))
newposxy = self.pos
self.updateLocalMap(oldposxy, newposxy)
# Check if SensorFly has collided
# TODO: Fix the collision coordinates
self.hasCollided = sf.hasCollided
def updateLocalMap(self, oldposxy, newposxy):
'''
Update covered local map
'''
# Mark new position as covered
# TODO: Add coverage to all cells along path
opos = self.localMap.xytocell(oldposxy[0],oldposxy[1])
npos = self.localMap.xytocell(newposxy[0],newposxy[1])
self.pos = npos
self.lastpos = opos
self.localMap.markMap(npos[0],npos[1],1)
