def __init__(self, nb):
"""Create a sensor node with undefined position
nb -- numerical identifier used to name the node
"""
name = "sensor" + str(nb)
UWNode.__init__(self, name)
self.calculator = UPSCalculator()
self.timeout = float('inf')
self.positionEstimate = None
class LSLSNode(UWNode):
"""All-purpose node localizing itself using the LSLS scheme"""
def __init__(self, id, position = (-1,-1,0), localized = False):
"""Create a node
name -- string identifying the node
position -- X,Y,Z coordinates of the node (m,m,m) (default -1,-1,0)
the default coordinates will be out of bounds of the simulation space, forcing a random assignment
"""
name = "node" + "".join(str(id).split()) # making sure the name does not contain any whitespace
UWNode.__init__(self, name, position)
self.timer = float('inf')
self.beaconCount = 0
self.tdoaCalc = None
self.master = []
if localized:
self.positionEstimate = position
# self.errorEstimate = 0
self.status = "LOCALIZED"
self.level = 1
else:
self.positionEstimate = None
# self.errorEstimate = -1
self.status = "UNLOCALIZED"
self.level = 0
def tick(self, time):
"""Function called every tick, lets the node perform operations
time -- date of polling (s)
Returns a string to be broadcast (if the string is empty, it is not broadcast)
"""
subject = ""
data = []
if time > self.timer:
if self.status == "UNLOCALIZED":
pass
elif self.status == "LISTENING":
pass
elif self.status == "LOCALIZED":
pass
elif self.status == "CANDIDATE":
self.status = "CONFIRMING"
self.timer = time + 2 * self.standardTimer()
subject = "confirm"
parent, d = self.master
data = [self.level, self.candidateTimer(d), parent]
elif self.status == "CONFIRMING":
self.status = "ANCHOR"
self.timer = float('inf') if self.level > 0 else time + (3 * LSLS_WAITFACTOR + 10) * self.standardTimer()
subject = "anchor"
parent, d = self.master
x, y, z = self.positionEstimate
data = [self.level, x, y, z, parent]
elif self.status == "ANCHOR":
subject = "beacon"
data = [self.beaconCount, self.level, time - self.timer]
if self.beaconCount == UPS_NUMBER - 1:
self.status = "LOCALIZED"
self.level = 1
self.timer = float('inf')
elif self.level == 0:
self.beaconCount += 1
self.timer += UPS_PERIOD
else:
self.timer = float('inf')
if len(subject) > 0:
return " ".join([self.name, subject] + [ str(e) for e in data ])
else:
return ""
def receive(self, time, message):
"""Function called when a message broadcast by another node arrives at the node
time -- date of reception (s)
message -- message received
Never transmits
"""
sp = message.split()
sender = sp[0]
subject = sp[1]
data = sp[2:]
if subject == "anchor":
[level, x, y, z, parent] = data
level = int(level)
x = float(x)
y = float(y)
z = float(z)
if self.status == "UNLOCALIZED":
if level == 0:
self.master.append([(sender, (x,y,z))])
else:
for chain in self.master:
if len(chain) == level and chain[-1][0] == parent:
chain.append((sender, (x,y,z)))
if len(chain) == 4:
self.status = "LISTENING"
self.tdoaCalc = UPSCalculator()
for i in xrange(4):
a, position = chain[i]
self.tdoaCalc.addAnchor(i, position)
self.master = chain
elif self.status == "LISTENING":
pass
elif self.status == "LOCALIZED":
d = distance(self.positionEstimate, (x,y,z))
if self.level == level + 1 and d <= LSLS_SUBRANGE:
# LOCALIZED node received a "anchor" message of lower level: becomes candidate
self.status = "CANDIDATE"
self.master = (sender, d)
self.timer = time + self.candidateTimer(d)
elif self.status == "CANDIDATE":
d = distance(self.positionEstimate, (x,y,z))
if level == self.level + 1 and d <= LSLS_SUBRANGE:
# CANDIDATE node received a "anchor" message of lower level: consider switching
t = time + self.candidateTimer(d)
if t < self.timer:
self.master = (sender, d)
self.timer = t
elif level == self.level and parent == self.master[0] and d <= LSLS_SUBRANGE:
# CANDIDATE node received a concurrent "anchor" message: become next-level candidate, or reset to LOCALIZED if the chain is complete
if self.level == 3:
self.status = "LOCALIZED"
self.level = 1
self.timer = float('inf')
else:
self.level += 1
self.master = (sender, d)
self.timer = time + self.candidateTimer(d)
elif self.status == "CONFIRMING":
pass # it should not be possible to receive a "anchor" message with the same parent during the confirmation stage
elif self.status == "ANCHOR":
pass
elif subject == "confirm":
[level, f, parent] = data
level = int(level)
f = float(f)
if self.status == "UNLOCALIZED":
pass
elif self.status == "LISTENING":
pass
elif self.status == "LOCALIZED":
pass
elif self.status == "CANDIDATE":
if level == self.level and parent == self.master[0]:
# CANDIDATE node received a concurrent "confirm" message: abandon, prepare for next round
self.status = "LOCALIZED"
self.level = (self.level % 3) + 1
self.timer = float('inf')
elif self.status == "CONFIRMING":
if level == self.level and parent == self.master[0]:
# CONFIRMING node received a concurrent "confirm" message: consider abandoning
if self.candidateTimer(self.master[1]) > f:
self.status = "LOCALIZED"
self.level = (self.level % 3) + 1
self.timer = float('inf')
elif self.status == "ANCHOR":
pass
elif subject == "beacon":
[count, level, delay] = data
count = int(count)
level = int(level)
delay = float(delay)
if self.status == "UNLOCALIZED":
self.master = []
elif self.status == "LISTENING":
if self.master[level][0] == sender:
self.tdoaCalc.addDataPoint(level, count, (time, delay))
if level == 3 and count == UPS_NUMBER - 1:
# beacon sequence finished, LISTENING node tries to calculate its position
# import json
# print json.dumps(self.tdoaCalc.dataArchive, sort_keys=True, indent=4)
msg, position = self.tdoaCalc.getPosition()
if msg != "ok":
# localization failed, revert to UNLOCALIZED status
self.status = "UNLOCALIZED"
self.master = []
else:
# localization successful, become CANDIDATE level 0
self.positionEstimate = position
# self.errorEstimate = e
self.status = "CANDIDATE"
self.level = 0
# calculate the anchor center
center = sum([ np.array(p) for a,p in self.master ]) / 4
d = distance(self.positionEstimate, center)
self.master = ("master", d)
self.timer = time + self.candidateTimer(d)
elif self.status == "LOCALIZED":
self.level = 1
elif self.status == "CANDIDATE":
pass
elif self.status == "CONFIRMING":
pass
elif self.status == "ANCHOR":
parent, d = self.master
if parent == sender and self.level == level + 1:
self.timer = time - d/SND_SPEED - delay # trigger a beacon at next tick, and indicate the time origin to use
self.beaconCount = count
return ""
def standardTimer(self):
"""A duration equal to the max transmission range divided by the speed of sound.
Multiples of this are used as timers for various stages
Returns: time (s)
"""
r = float(SIM_RANGE)
v = float(SND_SPEED)
return r/v
def candidateTimer(self, d):
"""The timer used during the "CANDIDATE" stage, depends on the position of the parent anchor and the candidate level
d -- distance the parent anchor
Returns: time (s)
"""
k = LSLS_WAITFACTOR
r = float(SIM_RANGE)
v = float(SND_SPEED)
# if self.level == 0:
# return k * (r - d) / v
# else:
# return k * (r - 4*d + 4*d*d/r) / v
return k * (r - 2*d) / v
def makeMaster(self):
"""Makes the node start the simulation as a master anchor node
Should be called on a single localized node
"""
self.status = "CONFIRMING" # necessary to trigger the anchor message
self.level = 0
self.master = ("master", 0)
self.timer = -1
def display(self, plot):
"""Displays a representation of the node in a 3D plot
plot -- matplotlib plot in which the node must display itself
"""
x, y, z = self.position
color, mark = {
"UNLOCALIZED": ("black", 'v'),
"LISTENING": ("blue", 'v'),
"LOCALIZED": ("orange", '^'),
"CANDIDATE": ("orange", 's'),
"CONFIRMING": ("orange", 's'),
"ANCHOR": ("red", 's')
}[self.status]
plot.scatter(x, y, z, c=color, marker=mark, lw=0)
if self.positionEstimate is not None:
ex, ey, ez = self.positionEstimate
plot.scatter(ex, ey, ez, c=color, marker='+')
# plot.scatter(ex, ey, ez, c=(0,0,0,0.2), marker='o', lw=0, s=20*self.errorEstimate)
plot.plot([x,ex], [y,ey], [z,ez], 'k:')
def receive(self, time, message):
"""Function called when a message broadcast by another node arrives at the node
time -- date of reception (s)
message -- message received
Never transmits
"""
sp = message.split()
sender = sp[0]
subject = sp[1]
data = sp[2:]
if subject == "anchor":
[level, x, y, z, parent] = data
level = int(level)
x = float(x)
y = float(y)
z = float(z)
if self.status == "UNLOCALIZED":
if level == 0:
self.master.append([(sender, (x,y,z))])
else:
for chain in self.master:
if len(chain) == level and chain[-1][0] == parent:
chain.append((sender, (x,y,z)))
if len(chain) == 4:
self.status = "LISTENING"
self.tdoaCalc = UPSCalculator()
for i in xrange(4):
a, position = chain[i]
self.tdoaCalc.addAnchor(i, position)
self.master = chain
elif self.status == "LISTENING":
pass
elif self.status == "LOCALIZED":
d = distance(self.positionEstimate, (x,y,z))
if self.level == level + 1 and d <= LSLS_SUBRANGE:
# LOCALIZED node received a "anchor" message of lower level: becomes candidate
self.status = "CANDIDATE"
self.master = (sender, d)
self.timer = time + self.candidateTimer(d)
elif self.status == "CANDIDATE":
d = distance(self.positionEstimate, (x,y,z))
if level == self.level + 1 and d <= LSLS_SUBRANGE:
# CANDIDATE node received a "anchor" message of lower level: consider switching
t = time + self.candidateTimer(d)
if t < self.timer:
self.master = (sender, d)
self.timer = t
elif level == self.level and parent == self.master[0] and d <= LSLS_SUBRANGE:
# CANDIDATE node received a concurrent "anchor" message: become next-level candidate, or reset to LOCALIZED if the chain is complete
if self.level == 3:
self.status = "LOCALIZED"
self.level = 1
self.timer = float('inf')
else:
self.level += 1
self.master = (sender, d)
self.timer = time + self.candidateTimer(d)
elif self.status == "CONFIRMING":
pass # it should not be possible to receive a "anchor" message with the same parent during the confirmation stage
elif self.status == "ANCHOR":
pass
elif subject == "confirm":
[level, f, parent] = data
level = int(level)
f = float(f)
if self.status == "UNLOCALIZED":
pass
elif self.status == "LISTENING":
pass
elif self.status == "LOCALIZED":
pass
elif self.status == "CANDIDATE":
if level == self.level and parent == self.master[0]:
# CANDIDATE node received a concurrent "confirm" message: abandon, prepare for next round
self.status = "LOCALIZED"
self.level = (self.level % 3) + 1
self.timer = float('inf')
elif self.status == "CONFIRMING":
if level == self.level and parent == self.master[0]:
# CONFIRMING node received a concurrent "confirm" message: consider abandoning
if self.candidateTimer(self.master[1]) > f:
self.status = "LOCALIZED"
self.level = (self.level % 3) + 1
self.timer = float('inf')
elif self.status == "ANCHOR":
pass
elif subject == "beacon":
[count, level, delay] = data
count = int(count)
level = int(level)
delay = float(delay)
if self.status == "UNLOCALIZED":
self.master = []
elif self.status == "LISTENING":
if self.master[level][0] == sender:
self.tdoaCalc.addDataPoint(level, count, (time, delay))
if level == 3 and count == UPS_NUMBER - 1:
# beacon sequence finished, LISTENING node tries to calculate its position
# import json
# print json.dumps(self.tdoaCalc.dataArchive, sort_keys=True, indent=4)
msg, position = self.tdoaCalc.getPosition()
if msg != "ok":
# localization failed, revert to UNLOCALIZED status
self.status = "UNLOCALIZED"
self.master = []
else:
# localization successful, become CANDIDATE level 0
self.positionEstimate = position
# self.errorEstimate = e
self.status = "CANDIDATE"
self.level = 0
# calculate the anchor center
center = sum([ np.array(p) for a,p in self.master ]) / 4
d = distance(self.positionEstimate, center)
self.master = ("master", d)
self.timer = time + self.candidateTimer(d)
elif self.status == "LOCALIZED":
self.level = 1
elif self.status == "CANDIDATE":
pass
elif self.status == "CONFIRMING":
pass
elif self.status == "ANCHOR":
parent, d = self.master
if parent == sender and self.level == level + 1:
self.timer = time - d/SND_SPEED - delay # trigger a beacon at next tick, and indicate the time origin to use
self.beaconCount = count
return ""
def receive(self, time, message): """Function called when a message broadcast by another node arrives at the node time -- date of reception (s) message -- message received """ words = message.split() sender = words[0] subject = words[1] data = words[2:] if subject == "position": x = float(data[0]) y = float(data[1]) z = float(data[2]) position = np.array([x, y, z]) error = float(data[3]) # if node is unlocalized, attempt to find a better anchor set if self.status in ["UP", "UA"]: self.findAnchors(sender, position, error) # add to the list of neighbor self.neighbors[sender] = (position, error) # revert to "unlocalized-passive" if needed if self.status == "UA" and time/RLS_TIMESLOT > self.slotTimer - RLSNode.slotNumber/2: self.status = "UP" if subject == "request": if self.status == "LR" and self.name in data: i = data.index(self.name) master = data[i-1 % 4] if master not in self.neighbors: return "" self.status = "A" self.anchorLevel = i self.anchorMaster = master p, e = self.neighbors[master] d = distance(self.position, p) self.masterDelay = d / SND_SPEED if i == 0: self.beaconTime = time self.beaconCount = 1 if subject == "beacon": level = int(data[0]) count = int(data[1]) delay = float(data[2]) if len(data) > 3: x = float(data[3]) y = float(data[4]) z = float(data[5]) e = float(data[6]) self.neighbors[sender] = (np.array([x,y,z]), e) if self.status == "A": self.listeningTimer = time + 4 * RLS_TIMESLOT if sender == self.anchorMaster: if self.anchorLevel == 0: self.beaconCount += 1 self.beaconTime = time else: self.beaconCount = count self.beaconTime = time - self.masterDelay - delay else: if self.status == "UA": self.status = "UP" self.listeningTimer = time + 2 * RLS_TIMESLOT # first beacon: new calculator if count == 1 and level == 0: self.tdoaCalc = UPSCalculator() elif self.tdoaCalc is None: return "" # first cycle: register anchors if count == 1: position, error = self.neighbors[sender] self.tdoaCalc.addAnchor(sender, position) self.anchorErrors[level] = error else: if len(self.tdoaCalc.anchors) < 4: self.tdoaCalc = None return "" # all cycles: register data self.tdoaCalc.addDataPoint(sender, count, (time, delay)) # final beacon: calculate position if count == UPS_NUMBER and level == 3: msg, position = self.tdoaCalc.getPosition() self.tdoaCalc = None print self.name + " calculating: " + msg print position if msg == "ok": x, y, z = position error = 1 + max(self.anchorErrors) self.positionEstimates.append((x,y,z, error)) if self.status in ["UP", "UA"]: self.status = "LN" if self.status == "LR": self.update = True return ""
class RLSNode(UWNode):
"""Node class implementing the "reactive localization scheme"""
slotNumber = 0
def __init__(self, id, position = (-1,-1,0), localized = False):
"""Create a node
id -- unique number identifying the node and its time slot
position -- X,Y,Z coordinates of the node (m,m,m) (default -1,-1,0)
the default coordinates will be out of bounds of the simulation space, forcing a random assignment
"""
name = "node-" + str(id)
UWNode.__init__(self, name, position)
# common attributes
self.message = None
self.status = "LN" if localized else "UP"
self.slotTimer = id
RLSNode.slotNumber = max(id+1, RLSNode.slotNumber)
# neighbor registration
self.neighbors = {}
# localization
self.listeningTimer = 0
self.tdoaCalc = None
self.anchorErrors = [0, 0, 0, 0]
# "unlocalized" status
self.bestAnchors = []
# "localized" status
x, y, z = position
self.positionEstimates = [(x, y, z, 0)] if localized else []
self.update = False
# "anchor" status
self.anchorLevel = None
self.anchorMaster = None
self.masterDelay = None
self.beaconTime = None
self.beaconCount = None
def tick(self, time):
"""Function called every tick, lets the node perform operations
time -- date of polling (s)
Returns a string to be broadcast (if the string is empty, it is not broadcast)
"""
if self.status == "A" and self.beaconTime is not None:
if self.beaconCount == UPS_NUMBER:
self.status = "LR"
delay = time - self.beaconTime
self.beaconTime = None
beacon = self.name + " beacon " + str(self.anchorLevel) + " " + str(self.beaconCount) + " " + str(delay)
if self.update:
x, y, z, e = self.getPosition()
beacon += " " + str(x) + " " + str(y) + " " + str(z) + " " + str(e)
self.update = False
return beacon
if time / RLS_TIMESLOT > self.slotTimer:
self.slotTimer += RLSNode.slotNumber
print str(time) + " " + self.name + " ping " + self.status
if self.status == "UP" and len(self.bestAnchors) > 0:
self.status = "UA"
return ""
if time > self.listeningTimer:
if self.status == "UA":
s, n0, n1, n2, n3 = heappop(self.bestAnchors)
print "score:", s
if len(self.bestAnchors) == 0:
self.status = "UP"
return self.name + " request " + " ".join([n0, n1, n2, n3])
if self.status == "LN":
self.status = "LR"
x, y, z, e = self.getPosition()
return self.name + " position " + str(x) + " " + str(y) + " " + str(z) + " " + str(e)
if self.status == "A":
# anchor is orphaned
self.status = "LR"
return ""
def receive(self, time, message):
"""Function called when a message broadcast by another node arrives at the node
time -- date of reception (s)
message -- message received
"""
words = message.split()
sender = words[0]
subject = words[1]
data = words[2:]
if subject == "position":
x = float(data[0])
y = float(data[1])
z = float(data[2])
position = np.array([x, y, z])
error = float(data[3])
# if node is unlocalized, attempt to find a better anchor set
if self.status in ["UP", "UA"]:
self.findAnchors(sender, position, error)
# add to the list of neighbor
self.neighbors[sender] = (position, error)
# revert to "unlocalized-passive" if needed
if self.status == "UA" and time/RLS_TIMESLOT > self.slotTimer - RLSNode.slotNumber/2:
self.status = "UP"
if subject == "request":
if self.status == "LR" and self.name in data:
i = data.index(self.name)
master = data[i-1 % 4]
if master not in self.neighbors:
return ""
self.status = "A"
self.anchorLevel = i
self.anchorMaster = master
p, e = self.neighbors[master]
d = distance(self.position, p)
self.masterDelay = d / SND_SPEED
if i == 0:
self.beaconTime = time
self.beaconCount = 1
if subject == "beacon":
level = int(data[0])
count = int(data[1])
delay = float(data[2])
if len(data) > 3:
x = float(data[3])
y = float(data[4])
z = float(data[5])
e = float(data[6])
self.neighbors[sender] = (np.array([x,y,z]), e)
if self.status == "A":
self.listeningTimer = time + 4 * RLS_TIMESLOT
if sender == self.anchorMaster:
if self.anchorLevel == 0:
self.beaconCount += 1
self.beaconTime = time
else:
self.beaconCount = count
self.beaconTime = time - self.masterDelay - delay
else:
if self.status == "UA":
self.status = "UP"
self.listeningTimer = time + 2 * RLS_TIMESLOT
# first beacon: new calculator
if count == 1 and level == 0:
self.tdoaCalc = UPSCalculator()
elif self.tdoaCalc is None:
return ""
# first cycle: register anchors
if count == 1:
position, error = self.neighbors[sender]
self.tdoaCalc.addAnchor(sender, position)
self.anchorErrors[level] = error
else:
if len(self.tdoaCalc.anchors) < 4:
self.tdoaCalc = None
return ""
# all cycles: register data
self.tdoaCalc.addDataPoint(sender, count, (time, delay))
# final beacon: calculate position
if count == UPS_NUMBER and level == 3:
msg, position = self.tdoaCalc.getPosition()
self.tdoaCalc = None
print self.name + " calculating: " + msg
print position
if msg == "ok":
x, y, z = position
error = 1 + max(self.anchorErrors)
self.positionEstimates.append((x,y,z, error))
if self.status in ["UP", "UA"]:
self.status = "LN"
if self.status == "LR":
self.update = True
return ""
def display(self, plot):
"""Displays a representation of the node in a 3D plot
plot -- matplotlib plot in which the node must display itself
"""
x, y, z = self.position
color, mark = {
"UP": ("grey", 'v'),
"UA": ("black", 'v'),
"LN": ("blue", '^'),
"LR": ("cyan", '^'),
"A": ("red", 's')
}[self.status]
plot.scatter(x, y, z, c=color, marker=mark, lw=0)
if len(self.positionEstimates) > 0:
ex, ey, ez, ee = self.getPosition()
plot.scatter(ex, ey, ez, c=color, marker='+')
plot.scatter(ex, ey, ez, c=(0,0,0,0.2), marker='o', lw=0, s=20*ee)
plot.plot([x,ex], [y,ey], [z,ez], 'k:')
def findAnchors(self, newNode, position, error):
l = len(self.neighbors)
if l >= 3:
for n1, n2, n3 in combinations(self.neighbors.keys(), 3):
p1, e1 = self.neighbors[n1]
p2, e2 = self.neighbors[n2]
p3, e3 = self.neighbors[n3]
s = self.rateAnchors([position, p1, p2, p3], [error, e1, e2, e3])
if s > 0:
heappush(self.bestAnchors, (-s, newNode, n1, n2, n3))
# print self.name + " " + str(self.bestAnchors) + " " + str(score)
def rateAnchors(self, positions, errors):
# eliminate sets where nodes are too distant
for n1 in positions:
for n2 in positions:
if np.linalg.norm(n1-n2) > SIM_RANGE:
return 0
# calculate the score
a = positions[1] - positions[0]
b = positions[2] - positions[0]
c = positions[3] - positions[0]
shapeRating = abs(np.dot(a, np.cross(b, c)))
errorRating = 1 + sum(errors)
return shapeRating / errorRating
def getPosition(self):
# takes the estimate with the lowest error
sx = 0
sy = 0
sz = 0
se = float('inf')
for x, y, z, e in self.positionEstimates:
if e < se:
sx = x
sy = y
sz = z
se = e
return sx, sy, sz, se
class SensorNode(UWNode):
"""Node that does not know its position, and calculates it by listening to the beacons"""
def __init__(self, nb):
"""Create a sensor node with undefined position
nb -- numerical identifier used to name the node
"""
name = "sensor" + str(nb)
UWNode.__init__(self, name)
self.calculator = UPSCalculator()
self.timeout = float('inf')
self.positionEstimate = None
# self.errorEstimate = 0
def tick(self, time):
"""Function called every tick, lets the node perform operations
time -- date of polling (s)
If the sensor has received all four beacons, calculates and log its position
Never transmits
"""
if time >= self.timeout:
error, position = self.calculator.getPosition()
if error != "ok":
print self.name + " could not find its position: " + error
print " actual position: " + "%.3f, %.3f, %.3f" % self.position
else:
x, y, z = position
print self.name + " found position: " + "%.3f, %.3f, %.3f" % (
x, y, z)
print " actual position: " + "%.3f, %.3f, %.3f" % self.position
print " error: " + "%.3f" % distance(
self.position, position)
# print " error estimate: " + "%.3f" % e
self.positionEstimate = position
# self.errorEstimate = e
self.timeout = float('inf')
return ""
def receive(self, time, message):
"""Function called when a message broadcast by another node arrives at the node
time -- date of reception (s)
message -- message received
Never transmits
"""
# print "{:6}".format("%.3f" % time) + " -- " + self.name + " received beacon: " + message
beaconCount = int(message.split()[0])
anchor = int(message.split()[1])
x, y, z, delay = [float(i) for i in message.split()[2:6]]
self.calculator.addAnchor(anchor, (x, y, z))
self.calculator.addDataPoint(anchor, beaconCount, (time, delay))
self.timeout = time + 5 # arbitrary 5-second timeout
return ""
def display(self, plot):
"""Displays a representation of the node in a 3D plot
plot -- matplotlib plot in which the node must display itself
"""
x, y, z = self.position
if self.positionEstimate is None:
plot.scatter(x, y, z, c='r', marker='^', lw=0)
else:
ex, ey, ez = self.positionEstimate
plot.scatter(x, y, z, c='b', marker='^', lw=0)
plot.scatter(ex, ey, ez, c='k', marker='+')
# plot.scatter(ex, ey, ez, c=(0,0,1,0.2), marker='o', lw=0, s=50*self.errorEstimate)
plot.plot([x, ex], [y, ey], [z, ez], 'k:')
def tick(self, time):
"""Function called every tick, lets the node perform operations
time -- date of polling (s)
Returns a string to be broadcast (if the string is empty, it is not broadcast)
"""
if time / RLS_TIMESLOT > self.slotTimer:
self.slotTimer += HRLSNode.slotNumber
timeslotOpen = True
# print self.name, self.status[0] + "/" + self.status[1]
else:
timeslotOpen = False
if self.status[0] == "UNLOCALIZED":
if self.status[1] == "idle":
if timeslotOpen and len(self.bestAnchors) > 0:
self.status[1] = "requesting"
self.timestamp = time + 2 * RLS_TIMESLOT
elif self.status[1] == "requesting":
if timeslotOpen and time > self.timestamp:
self.status[1] = "idle"
s, n0, n1, n2, n3 = heappop(self.bestAnchors)
return self.name + " request " + " ".join([n0, n1, n2, n3])
elif self.status[0] == "LOCALIZED":
if self.status[1] == "new":
if timeslotOpen and time > self.timestamp:
self.status[1] = "ready"
x, y, z = self.getPosition()
return self.name + " position " + str(x) + " " + str(
y) + " " + str(z)
elif self.status[1] == "confirming":
if time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
elif self.status[1] == "toa":
if time > self.timestamp + 2 * RLS_TIMESLOT:
msg, position = self.calculator.getPosition()
if msg == "ok":
print self.name, msg, position, distance(
position, self.position)
self.status = ["ANCHOR", "confirming"]
self.timestamp = time + RLS_TIMESLOT
self.positionEstimates = [position]
x, y, z = position
return self.name + " anchor " + " " + str(
x) + " " + str(y) + " " + str(z)
else:
self.status[1] = "ready"
elif self.status[0] == "ANCHOR":
if self.status[1] == "confirming":
if time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
elif self.status[1] == "active":
if time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
elif self.status[1] == "init":
if timeslotOpen:
self.status[1] = "ready"
x, y, z = self.getPosition()
return self.name + " anchor " + str(x) + " " + str(
y) + " " + str(z)
if self.status[1] == "confirming":
# similar behavior regardless of primary status
if len(self.subAnchors) == 0:
if self.status[0] == "ANCHOR":
self.status[1] = "active"
if self.anchorLevel == 0:
return self.name + " beacon 0 1 0"
else:
self.status[1] = "toa"
self.calculator = TOACalculator(self.getPosition())
self.timestamp = time
return self.name + " ping"
elif time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
return ""
def receive(self, time, message):
"""Function called when a message broadcast by another node arrives at the node
time -- date of reception (s)
message -- message received
Returns a string to be broadcast (if the string is empty, it is not broadcast)
"""
words = message.split()
sender = words[0]
subject = words[1]
data = words[2:]
if subject == "position":
# ALL: register the neighbor
# UNLOCALIZED: find new anchor sets
x = float(data[0])
y = float(data[1])
z = float(data[2])
position = np.array([x, y, z])
# add to the list of neighbor
self.neighbors[sender] = (False, position)
# if node is unlocalized, attempt to find a better anchor set, and revert to "idle" status
if self.status[0] == "UNLOCALIZED":
self.findAnchors(sender, position)
self.status[1] = "idle"
if subject == "anchor":
# ALL: register the neighbor
# UNLOCALIZED: update anchor ratings
# confirming: remove from the list of sub-anchors, if applicable
x = float(data[0])
y = float(data[1])
z = float(data[2])
position = np.array([x, y, z])
# add to the list of neighbor
self.neighbors[sender] = (True, position)
# if node is unlocalized, attempt to find a better anchor set, and revert to "idle" status
if self.status[0] == "UNLOCALIZED":
self.findAnchors(sender, position)
self.status[1] = "idle"
# remove from sub-anchors
while sender in self.subAnchors:
self.subAnchors.remove(sender)
if subject == "request":
# silence timer
if self.status[0] == "UNLOCALIZED" or self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
# /ready, concerned: transition to /confirming
# if level 0: transition to next state
if self.status[1] == "ready" and self.name in data:
for node in data:
if node not in self.neighbors and node != self.name:
return ""
i = data.index(self.name)
self.subAnchors = [
node for node in data[i + 1:]
if not self.neighbors[node][0]
]
self.anchorLevel = i
self.anchorMaster = data[(i - 1) % 4]
self.status[1] = "confirming"
self.timestamp = time + RLS_TIMESLOT
self.beaconCount = 1
if subject == "ping":
# silence & timeout
if self.status[0] == "UNLOCALIZED" or self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
if self.status[1] == "confirming" or self.status[1] == "active":
self.timestamp = time + 3 * RLS_TIMESLOT
# if ANCHOR: send "ack"
if self.status[0] == "ANCHOR":
return self.name + " ack " + sender + " " + str(SIM_TICK)
if subject == "ack":
# silence timer
if self.status[0] == "UNLOCALIZED" or self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
# if concerned: register TOA data
recipient = data[0]
delay = float(data[1])
if self.status[1] == "toa":
if self.name == recipient:
self.calculator.addAnchor(sender,
self.neighbors[sender][1])
self.calculator.addDataPoint(
sender, 0, (time - self.timestamp, delay))
if subject == "beacon":
# silence & timeout
if self.status[0] == "UNLOCALIZED":
self.status[1] = "idle"
if self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
if self.status[1] == "active":
self.timestamp = time + 2 * RLS_TIMESLOT
# not ANCHOR: register TDOA data
# ANCHOR/active, concerned: send "beacon"
level = int(data[0])
count = int(data[1])
delay = float(data[2])
if self.status == ["ANCHOR", "active"]:
# check if concerned
if sender != self.anchorMaster:
return ""
if (level + 1) % 4 != self.anchorLevel:
# should not happen!
self.status[1] = "ready"
return ""
if self.anchorLevel == 0:
self.beaconCount += 1
newDelay = 0
else:
self.beaconCount = count
timeToMaster = distance(
self.getPosition(),
self.neighbors[sender][1]) / SND_SPEED
newDelay = delay + timeToMaster + SIM_TICK
if self.beaconCount == UPS_NUMBER:
self.status[1] = "ready"
return self.name + " beacon " + str(
self.anchorLevel) + " " + str(
self.beaconCount) + " " + str(newDelay)
elif self.status[0] != "ANCHOR" and self.status[1] not in [
"confirming", "toa"
]:
if count == 1 and level == 0:
self.calculator = UPSCalculator()
elif self.calculator is None:
return ""
if count == 1 and len(
self.calculator.anchors
) == level and sender not in self.calculator.anchors:
self.calculator.addAnchor(sender,
self.neighbors[sender][1])
elif len(self.calculator.anchors) < 4:
self.calculator = None
return ""
if sender != self.calculator.anchors[level]:
return ""
# register data
self.calculator.addDataPoint(sender, count, (time, delay))
# if finished, do calculation
if level == 3 and count == UPS_NUMBER:
msg, position = self.calculator.getPosition()
print self.name, msg, position, distance(
position, self.position)
if msg == "ok":
self.positionEstimates.append(position)
if self.status[0] == "UNLOCALIZED":
self.status = ["LOCALIZED", "new"]
self.calculator = None
return ""
class HRLSNode(UWNode):
"""Node class implementing the "reactive localization scheme"""
slotNumber = 0 # number of time slots in a full cycle
def __init__(self, id, position=(-1, -1, 0), localized=False):
"""Create a node
id -- unique number identifying the node and its time slot
position -- X,Y,Z coordinates of the node (m,m,m) (default -1,-1,0)
the default coordinates will be out of bounds of the simulation space, forcing a random assignment
"""
name = "node-" + str(id)
UWNode.__init__(self, name, position)
# common attributes
self.status = ["ANCHOR", "init"] if localized else [
"UNLOCALIZED", "idle"
] # primary and secondary status of the node
self.timestamp = 0 # multipurpose time stamp, meaning depends on current state (unit: s)
# UNLOCALIZED: silence after certain messages
# /confirming, ANCHOR/active: timeout
# LOCALIZED/toa: time origin
self.slotTimer = id # timer indicating the next timeslot (unit: timeslot length)
HRLSNode.slotNumber = max(id + 1, HRLSNode.slotNumber)
# neighbor registration
self.neighbors = {
} # associates a pair bool,position to each neighbor's name
# the boolean indicates if the neighbor is an anchor (precisely located)
# localization
self.calculator = None # can be either TDOA or TOA calculator
# "unlocalized" status
self.bestAnchors = []
# "localized" status
self.positionEstimates = [np.array(position)] if localized else []
# "anchor" status
self.subAnchors = [
] # anchors coming after in the beaconing cycle that have not anchor rank yet
self.anchorLevel = 0 # place in the beaconing
self.anchorMaster = None # anchor coming just before in the beaconing cycle
self.beaconCount = 1 # counts the beaconing series
def tick(self, time):
"""Function called every tick, lets the node perform operations
time -- date of polling (s)
Returns a string to be broadcast (if the string is empty, it is not broadcast)
"""
if time / RLS_TIMESLOT > self.slotTimer:
self.slotTimer += HRLSNode.slotNumber
timeslotOpen = True
# print self.name, self.status[0] + "/" + self.status[1]
else:
timeslotOpen = False
if self.status[0] == "UNLOCALIZED":
if self.status[1] == "idle":
if timeslotOpen and len(self.bestAnchors) > 0:
self.status[1] = "requesting"
self.timestamp = time + 2 * RLS_TIMESLOT
elif self.status[1] == "requesting":
if timeslotOpen and time > self.timestamp:
self.status[1] = "idle"
s, n0, n1, n2, n3 = heappop(self.bestAnchors)
return self.name + " request " + " ".join([n0, n1, n2, n3])
elif self.status[0] == "LOCALIZED":
if self.status[1] == "new":
if timeslotOpen and time > self.timestamp:
self.status[1] = "ready"
x, y, z = self.getPosition()
return self.name + " position " + str(x) + " " + str(
y) + " " + str(z)
elif self.status[1] == "confirming":
if time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
elif self.status[1] == "toa":
if time > self.timestamp + 2 * RLS_TIMESLOT:
msg, position = self.calculator.getPosition()
if msg == "ok":
print self.name, msg, position, distance(
position, self.position)
self.status = ["ANCHOR", "confirming"]
self.timestamp = time + RLS_TIMESLOT
self.positionEstimates = [position]
x, y, z = position
return self.name + " anchor " + " " + str(
x) + " " + str(y) + " " + str(z)
else:
self.status[1] = "ready"
elif self.status[0] == "ANCHOR":
if self.status[1] == "confirming":
if time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
elif self.status[1] == "active":
if time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
elif self.status[1] == "init":
if timeslotOpen:
self.status[1] = "ready"
x, y, z = self.getPosition()
return self.name + " anchor " + str(x) + " " + str(
y) + " " + str(z)
if self.status[1] == "confirming":
# similar behavior regardless of primary status
if len(self.subAnchors) == 0:
if self.status[0] == "ANCHOR":
self.status[1] = "active"
if self.anchorLevel == 0:
return self.name + " beacon 0 1 0"
else:
self.status[1] = "toa"
self.calculator = TOACalculator(self.getPosition())
self.timestamp = time
return self.name + " ping"
elif time > self.timestamp:
print self.name, self.status, "timeout", self.timestamp
self.status[1] = "ready"
return ""
def receive(self, time, message):
"""Function called when a message broadcast by another node arrives at the node
time -- date of reception (s)
message -- message received
Returns a string to be broadcast (if the string is empty, it is not broadcast)
"""
words = message.split()
sender = words[0]
subject = words[1]
data = words[2:]
if subject == "position":
# ALL: register the neighbor
# UNLOCALIZED: find new anchor sets
x = float(data[0])
y = float(data[1])
z = float(data[2])
position = np.array([x, y, z])
# add to the list of neighbor
self.neighbors[sender] = (False, position)
# if node is unlocalized, attempt to find a better anchor set, and revert to "idle" status
if self.status[0] == "UNLOCALIZED":
self.findAnchors(sender, position)
self.status[1] = "idle"
if subject == "anchor":
# ALL: register the neighbor
# UNLOCALIZED: update anchor ratings
# confirming: remove from the list of sub-anchors, if applicable
x = float(data[0])
y = float(data[1])
z = float(data[2])
position = np.array([x, y, z])
# add to the list of neighbor
self.neighbors[sender] = (True, position)
# if node is unlocalized, attempt to find a better anchor set, and revert to "idle" status
if self.status[0] == "UNLOCALIZED":
self.findAnchors(sender, position)
self.status[1] = "idle"
# remove from sub-anchors
while sender in self.subAnchors:
self.subAnchors.remove(sender)
if subject == "request":
# silence timer
if self.status[0] == "UNLOCALIZED" or self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
# /ready, concerned: transition to /confirming
# if level 0: transition to next state
if self.status[1] == "ready" and self.name in data:
for node in data:
if node not in self.neighbors and node != self.name:
return ""
i = data.index(self.name)
self.subAnchors = [
node for node in data[i + 1:]
if not self.neighbors[node][0]
]
self.anchorLevel = i
self.anchorMaster = data[(i - 1) % 4]
self.status[1] = "confirming"
self.timestamp = time + RLS_TIMESLOT
self.beaconCount = 1
if subject == "ping":
# silence & timeout
if self.status[0] == "UNLOCALIZED" or self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
if self.status[1] == "confirming" or self.status[1] == "active":
self.timestamp = time + 3 * RLS_TIMESLOT
# if ANCHOR: send "ack"
if self.status[0] == "ANCHOR":
return self.name + " ack " + sender + " " + str(SIM_TICK)
if subject == "ack":
# silence timer
if self.status[0] == "UNLOCALIZED" or self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
# if concerned: register TOA data
recipient = data[0]
delay = float(data[1])
if self.status[1] == "toa":
if self.name == recipient:
self.calculator.addAnchor(sender,
self.neighbors[sender][1])
self.calculator.addDataPoint(
sender, 0, (time - self.timestamp, delay))
if subject == "beacon":
# silence & timeout
if self.status[0] == "UNLOCALIZED":
self.status[1] = "idle"
if self.status[1] == "new":
self.timestamp = time + 2 * RLS_TIMESLOT
if self.status[1] == "active":
self.timestamp = time + 2 * RLS_TIMESLOT
# not ANCHOR: register TDOA data
# ANCHOR/active, concerned: send "beacon"
level = int(data[0])
count = int(data[1])
delay = float(data[2])
if self.status == ["ANCHOR", "active"]:
# check if concerned
if sender != self.anchorMaster:
return ""
if (level + 1) % 4 != self.anchorLevel:
# should not happen!
self.status[1] = "ready"
return ""
if self.anchorLevel == 0:
self.beaconCount += 1
newDelay = 0
else:
self.beaconCount = count
timeToMaster = distance(
self.getPosition(),
self.neighbors[sender][1]) / SND_SPEED
newDelay = delay + timeToMaster + SIM_TICK
if self.beaconCount == UPS_NUMBER:
self.status[1] = "ready"
return self.name + " beacon " + str(
self.anchorLevel) + " " + str(
self.beaconCount) + " " + str(newDelay)
elif self.status[0] != "ANCHOR" and self.status[1] not in [
"confirming", "toa"
]:
if count == 1 and level == 0:
self.calculator = UPSCalculator()
elif self.calculator is None:
return ""
if count == 1 and len(
self.calculator.anchors
) == level and sender not in self.calculator.anchors:
self.calculator.addAnchor(sender,
self.neighbors[sender][1])
elif len(self.calculator.anchors) < 4:
self.calculator = None
return ""
if sender != self.calculator.anchors[level]:
return ""
# register data
self.calculator.addDataPoint(sender, count, (time, delay))
# if finished, do calculation
if level == 3 and count == UPS_NUMBER:
msg, position = self.calculator.getPosition()
print self.name, msg, position, distance(
position, self.position)
if msg == "ok":
self.positionEstimates.append(position)
if self.status[0] == "UNLOCALIZED":
self.status = ["LOCALIZED", "new"]
self.calculator = None
return ""
def display(self, plot):
"""Displays a representation of the node in a 3D plot
plot -- matplotlib plot in which the node must display itself
"""
x, y, z = self.position
color, mark = {
"UNLOCALIZED": ("black", 'v'),
"LOCALIZED": ("cyan", '^'),
"ANCHOR": ("blue", '^')
}[self.status[0]]
plot.scatter(x, y, z, c=color, marker=mark, lw=0)
if len(self.positionEstimates) > 0:
ex, ey, ez = self.getPosition()
plot.scatter(ex, ey, ez, c=color, marker='+')
# plot.scatter(ex, ey, ez, c=(0,0,0,0.2), marker='o', lw=0, s=20*ee)
plot.plot([x, ex], [y, ey], [z, ez], 'k:')
def findAnchors(self, newNode, position):
# very inefficient function
self.bestAnchors = []
l = len(self.neighbors)
if l >= 4:
for n0, n1, n2, n3 in combinations(self.neighbors.keys(), 4):
s = self.rateAnchors([n0, n1, n2, n3])
if s > 0:
heappush(self.bestAnchors, (-s, n0, n1, n2, n3))
def rateAnchors(self, anchors):
isAnchor = [self.neighbors[node][0] for node in anchors]
positions = [self.neighbors[node][1] for node in anchors]
# eliminate sets where nodes are too distant
avgDist = 0
for n1 in positions:
for n2 in positions:
d = np.linalg.norm(n1 - n2)
if d > SIM_RANGE:
return 0
else:
avgDist += d
avgDist /= 12
# calculate the score
sizeRating = min(avgDist, SIM_RANGE / 2)
a = positions[1] - positions[0]
b = positions[2] - positions[0]
c = positions[3] - positions[0]
shapeRating = abs(np.dot(a, np.cross(b, c)))**(1 / 3) / avgDist
anchorRating = (1 + sum(isAnchor))**2
return sizeRating * shapeRating * anchorRating
def getPosition(self):
# average the estimates
return sum(self.positionEstimates) / len(self.positionEstimates)