コード例 #1
0
ファイル: ups.py プロジェクト: antoineppm/uasn-simulation
    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
コード例 #2
0
ファイル: lsls.py プロジェクト: antoineppm/uasn-simulation
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:')
コード例 #3
0
ファイル: lsls.py プロジェクト: antoineppm/uasn-simulation
	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 ""
コード例 #4
0
ファイル: rls.py プロジェクト: antoineppm/uasn-simulation
	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 ""
コード例 #5
0
ファイル: rls.py プロジェクト: antoineppm/uasn-simulation
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
コード例 #6
0
ファイル: ups.py プロジェクト: antoineppm/uasn-simulation
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:')
コード例 #7
0
ファイル: hrls.py プロジェクト: antoineppm/uasn-simulation
    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 ""
コード例 #8
0
ファイル: hrls.py プロジェクト: antoineppm/uasn-simulation
    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 ""
コード例 #9
0
ファイル: hrls.py プロジェクト: antoineppm/uasn-simulation
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)