def __init__(self, position, references, canvas=None):
"""Constructor"""
super().__init__()
self.started = True
self.references = references
self.currentposition = Position()
self.initialposition = position
self.targetposition = None
self.trajectory = []
self.route = []
self.inittime = time.time()
self.condition = measures_monitor.getcondition()
self.pointscondition = points_monitor.getcondition()
self.feedbackcondition = feedback_monitor.getcondition()
self.measures = []
self.state = STATES[0]
if canvas:
self.canvas = canvas
else:
self.canvas = None
self.feedbackcondition.acquire()
feedback_monitor.initialposition = self.currentposition
self.feedbackcondition.notify()
self.feedbackcondition.release()
def locate(self):
"""This method localizes the radionavigator using the measures it gets and offers the target direction"""
for mea in self.measures:
print(mea.getrssi(), mea.getuuid())
for ref in self.references:
print(ref.getx(), ref.gety())
print(self.currentposition, self.initialposition, self.targetposition)
self.system.setmeasures(self.measures)
self.currentposition = self.system.getpositionusingrssiranging(
self.references[0], self.references[1],
(self.currentposition.getx() + 1, self.currentposition.gety() + 1))
self.trajectory.append(self.currentposition)
if self.canvas and len(self.measures) > 2:
self.canvas.paint(self.currentposition.getx(),
self.currentposition.gety())
self.pointscondition.acquire()
points_monitor.enqueuepoint(
Position(x=self.currentposition.getx(),
y=self.currentposition.gety()))
self.pointscondition.notify()
self.pointscondition.release()
self.feedbackcondition.acquire()
feedback_monitor.enqueuepoint(
Position(x=self.currentposition.getx(),
y=self.currentposition.gety()))
self.feedbackcondition.notify()
self.feedbackcondition.release()
return True
def generatelinepath(initialposition, targetposition, steps):
"""This function generates a secuence of positions over a line; it returns steps+1 positions, including initial and
target points"""
distancestep = calculatedistance(initialposition, targetposition) / steps
if calculatedistance(initialposition, targetposition) == 0.0:
return [initialposition]
elif calculatedistance(initialposition, targetposition) <= distancestep:
return [initialposition, targetposition]
# 1.- The line y = mx + n between both points is the first equation.
# 2.- If the domain of definition between the final point and the initial point is partitioned into segments of
# equal length and the line's equation is evaluated in that set of points the path is obtained.
# 4.- The domain of definition could be either (yt - yi) or (xt - xi), due to its linearity.
xi = initialposition.getx()
yi = initialposition.gety()
xt = targetposition.getx()
yt = targetposition.gety()
m = (yt - yi) / (xt - xi)
n = -xi * m + yi
path = []
for i in range(steps):
x = xi + i * (xt - xi) / steps
position = Position(x=x, y=m * x + n)
print(position)
path.append(position)
path.append(targetposition)
return path
def projecttoground(position):
"""The method 'projects' a position to the ground """
if position:
newposition = Position(x=position.getx(), y=position.gety(), z=0)
return newposition
else:
return position
def getaproxpositionusingrssiranging(self, data, resolution=0.03):
"""This method aproximates a position using rssi values measured from certain reference positions"""
position = Position(x=0.0, y=0.0, z=0.0)
for i, reference in data["positions"]:
for reference in genius:
return position
def projecttoground(self, position):
"""The method 'projects' a position to the ground """
if position:
print("method projected", position.getx(), position.gety())
newposition = Position(x=position.getx(), y=position.gety(), z=0)
print(newposition)
return newposition
else:
return position
def run(self):
while True:
ran1 = random.uniform(0, 1) * 300
ran2 = random.uniform(0, 1) * 300
self.condition.acquire()
plotting_monitor.enquedata(Position(x=ran1, y=ran2))
print("New point")
self.condition.notify()
self.condition.release()
time.sleep(1)
def main():
"""Main execution"""
# Each iteration will radiodetermine a position
locatedpositions = []
# Probes are needed by the devices as a source of mesurements
probes = []
for pos in PROBES_POSITIONS:
probes.append(Probe(pos))
for i, pos in enumerate(TARGET_POSITIONS):
generators = []
for j, probe in enumerate(probes):
generator = MeasuresGenerator(MEASURE_TIMESTEP, UUID[j],
GENERATOR_MODES[1],
GAUSSIAN_NOISE_STATISTICS, F, G, pos,
[probe])
generators.append(generator)
radiolocator = Radiolocator(
probes, MEASURE_MODES[0], SYSTEM_MODES[0], F, G,
Position(x=pos.getx() + 1, y=pos.gety() + 1))
for generator in generators:
generator.start()
radiolocator.start()
radiolocator.join()
for generator in generators:
generator.join()
locatedpositions.append((i, radiolocator.getcalculatedpositions()))
"""
locatedpositions = [(0, Position(x=10, y=10)),
(1, Position(x=30, y=90)),
(2, Position(x=100, y=10)),
(3, Position(x=70, y=15)),
(4, Position(x=100, y=100)),
(5, Position(x=10, y=100)),
(6, Position(x=50, y=60))
]
"""
name = "id" + \
str(time.localtime().tm_year) + \
str(time.localtime().tm_mon) + \
str(time.localtime().tm_mday) + \
str(time.localtime().tm_hour) + \
str(time.localtime().tm_min) + \
str(time.localtime().tm_sec)
classmodel, dicmodel = model_generator.createmodel(name, locatedpositions)
print(classmodel)
print(dicmodel)
def getpositionusingrssiranging(self,
reference1,
reference2,
prediction=(1, 1)):
"""This method performs the calculate of position using time references"""
position = Position(x=0.0, y=0.0)
# Classify the input measures and averaging
self.classifymeasures()
self.averagemeasures()
if len(self.uuid) > 1:
x1 = reference1.getx()
y1 = reference1.gety()
x2 = reference2.getx()
y2 = reference2.gety()
rssi1 = self.averagedmeasures[0][1]
rssi2 = self.averagedmeasures[1][1]
dis1 = RSSIRanger.rangedistance(rssi1)
dis2 = RSSIRanger.rangedistance(rssi2)
# Solve the determination equations
def equations(p):
x, y = p
return (x - x1)**2 + (y - y1)**2 - dis1**2, (x - x2)**2 + (
y - y2)**2 - dis2**2
x, y = fsolve(equations, prediction)
position.setx(x)
position.sety(y)
return position
def getpositionusingtime(self):
"""This method performs the calculate of position using time references"""
position = Position()
# Classify the input measures and averaging
self.classifymeasures()
self.averagemeasures()
# Calculate average ranging
for i, uuid in enumerate(self.uuid):
sum = 0
for measure in self.classifiedmeasures[i]:
sum = sum + measure.getarrivaltime()
def main():
"""Main execution"""
# canvas = GUI()
navigator = Radionavigator(Position(x=0, y=0), [Position(x=200, y=0), Position(x=0, y=200)])
navigator.setinitialposition(Position(x=0, y=0))
navigator.settargetposition(Position(x=50, y=50))
references = [Position(x=100, y=0), Position(x=0, y=100)]
navigator.setreferences(references)
generator1 = MeasuresGenerator(uuid=1, mode="RADIONAVIGATOR")
generator2 = MeasuresGenerator(uuid=2, mode="RADIONAVIGATOR")
plot = Plot()
plot.start()
generator1.start()
generator2.start()
navigator.start()
def generatecirclepath(initialposition,
centerposition,
stepsperrevolution,
clockwise=True):
"""This function generates a secuence of positions over a line"""
radius = calculatedistance(initialposition, centerposition)
if radius <= 1.0:
return [initialposition]
if stepsperrevolution < 2:
return [initialposition]
# 1.- The center point defines a vector.
# 2.- The (x cos θ, y sen θ) vector rotates on it clockwise or counter clockwise from a relative angle that
# determines both initial point and center point
# 3.- The relative angle is arctan (yi-yc)/(xi-xc).
xi = initialposition.getx()
yi = initialposition.gety()
xc = centerposition.getx()
yc = centerposition.gety()
anglestep = 2 * pi / stepsperrevolution
offsetangle = atan((yi - yc) / (xi - xc))
path = []
for i in range(stepsperrevolution):
if clockwise:
x = radius * cos(-i * anglestep + offsetangle) + xc
y = radius * sin(-i * anglestep + offsetangle) + yc
else:
x = radius * cos(i * anglestep + offsetangle) + xc
y = radius * sin(i * anglestep + offsetangle) + yc
position = Position(x=x, y=y)
path.append(position)
return path
"""This file executes a demo of the system"""
from miso_beacon_radiodet.position import Position
from miso_beacon_radiodet.probe import Probe
from miso_beacon_demo.measures_generator import MeasuresGenerator
from miso_beacon_radiodet.miso_beacon_radiodet_nav.radiolocator import Radiolocator
from miso_beacon_model import model_generator
import time
# Demo configuration parameters
TARGET_POSITIONS = [ # Number of calculations
Position(x=60.0, y=20.0),
Position(x=60.0, y=60.0),
Position(x=20.0, y=60.0),
Position(x=20.0, y=20.0),
]
C = 299792458 # Speed of light
F = 2440000000 # 2400 - 2480 MHz
G = 1 # 2.16 dBi
# Measure generators configuration parameters
PROBES_POSITIONS = [ # Positions of device probes
Position(x=0.0, y=0.0),
Position(x=0.0, y=100.0),
]
MEASURE_TIMESTEP = 1
UUID = [ # Same length than PROBES_POSITIONS or greater
10,
20,
30,
"""This feedback monitor is access by devices and measures generators"""
from threading import Condition
from miso_beacon_radiodet.position import Position
points = []
condition = Condition()
initialposition = Position(x=0, y=0)
# Common functions
def getcondition():
"""This method is the concurrent manage condition getter"""
return condition
# Radiolocator feedback functions
radiolocatoridle = False
def isradiolocatoridle():
"""The getter of radiolocator's 'idle' flag"""
return radiolocatoridle
def setradiolocatoridle(flag):
"""The getter of radiolocator's 'idle' flag"""
global radiolocatoridle
radiolocatoridle = flag
from miso_beacon_model.miso_beacon_model_meta.graph_metamodel import GRAPH_METAMODEL
vertex = GRAPH_METAMODEL[0]
from miso_beacon_radiodet.position import Position
positions = [
Position(x=10, y=10),
Position(x=30, y=90),
Position(x=100, y=10),
Position(x=70, y=15),
Position(x=100, y=100),
Position(x=10, y=100),
Position(x=50, y=60)
]
locations = []
for i, pos in enumerate(positions):
locations.append((i, pos))
vertices = []
for loc in locations:
newvertice = vertex(loc[0], loc[1], [], [])
newvertice.setposition(loc[1])
vertices.append(newvertice)
from miso_beacon_ai.graph_functions import convexhullgrahamscan
convexhull, vertices, descartedvertices = convexhullgrahamscan(vertices)
print(convexhull)
print(vertices)
print(descartedvertices)
from miso_beacon_radiodet.miso_beacon_radiodet_loc.rho_rho_system import RhoRhoSystem
from miso_beacon_radiodet.measure import Measure
from miso_beacon_radiodet.position import Position
mea1 = Measure(uuid=7, rssi=50)
mea2 = Measure(uuid=6, rssi=50)
sys = RhoRhoSystem([mea1, mea2])
sys.classifymeasures()
sys.averagemeasures()
prediction = (-1, 1)
print(sys.getpositionusingrssi(Position(x=0, y=0), Position(x=0, y=2), prediction=prediction).getx(),
sys.getpositionusingrssi(Position(x=0, y=0), Position(x=0, y=2), prediction=prediction).gety())
from miso_beacon_radiodet.miso_beacon_range.rssi_ranger import RSSIRanger
print(RSSIRanger.rangedistance(50))
class Radionavigator(Thread):
def __init__(self, position, references, canvas=None):
"""Constructor"""
super().__init__()
self.started = True
self.references = references
self.currentposition = Position()
self.initialposition = position
self.targetposition = None
self.trajectory = []
self.route = []
self.inittime = time.time()
self.condition = measures_monitor.getcondition()
self.pointscondition = points_monitor.getcondition()
self.feedbackcondition = feedback_monitor.getcondition()
self.measures = []
self.state = STATES[0]
if canvas:
self.canvas = canvas
else:
self.canvas = None
self.feedbackcondition.acquire()
feedback_monitor.initialposition = self.currentposition
self.feedbackcondition.notify()
self.feedbackcondition.release()
# Getters and setters
def getcurrentposition(self):
"""Current position getter"""
return self.currentposition
def setcurrentposition(self, position):
"""Current position setter"""
self.currentposition = position
def gettargetposition(self):
"""Target position getter"""
return self.targetposition
def settargetposition(self, position):
"""Target position setter"""
self.targetposition = position
def getinitialposition(self):
"""Initial position getter"""
return self.initialposition
def setinitialposition(self, position=Position()):
"""Initial position setter"""
self.initialposition = position
def getreferences(self):
"""References getter"""
return self.references
def setreferences(self, references):
"""References setter"""
self.references = references
def isstarted(self):
"""Started flag getter"""
return self.started
def setstarted(self, started=True):
"""Started flag setter; True default"""
self.started = started
def gettrajectory(self):
"""Getter of trajectory"""
return self.trajectory
# Route projection
def projecttoground(self, position):
"""The method 'projects' a position to the ground """
if position:
print("method projected", position.getx(), position.gety())
newposition = Position(x=position.getx(), y=position.gety(), z=0)
print(newposition)
return newposition
else:
return position
# Journey control
def run(self):
"""Overwrite run method with the state machine of the navigator"""
time.sleep(random.uniform(0, 1))
while True:
print(self.state)
if self.state == "WAIT":
if self.started:
if self.initjourney():
self.pointscondition.acquire()
points_monitor.isarrived = False
self.pointscondition.notify()
self.pointscondition.release()
self.feedbackcondition.acquire()
feedback_monitor.isarrived = False
self.feedbackcondition.notify()
self.feedbackcondition.release()
self.state = "NEW_DATA"
elif self.state == "NEW_DATA":
if self.getnewdata():
self.state = "NO_LOCATED"
elif self.state == "NO_LOCATED":
if self.locate():
if self.currentposition:
self.state = "LOCATED"
elif self.state == "LOCATED":
print(self.currentposition)
if self.isarrived():
self.started = False
self.pointscondition.acquire()
points_monitor.isarrived = True
self.pointscondition.notify()
self.pointscondition.release()
self.feedbackcondition.acquire()
feedback_monitor.isarrived = True
self.feedbackcondition.notify()
self.feedbackcondition.release()
self.state = "WAIT"
else:
if self.isnewdata():
self.state = "NEW_DATA"
def initjourney(self):
"""This method sets the system to an initial position"""
self.inittime = time.time()
self.system = RhoRhoSystem()
return True
def targetpath(self):
"""This method calculates the target path that must be done for reaching it"""
pass
def locate(self):
"""This method localizes the radionavigator using the measures it gets and offers the target direction"""
for mea in self.measures:
print(mea.getrssi(), mea.getuuid())
for ref in self.references:
print(ref.getx(), ref.gety())
print(self.currentposition, self.initialposition, self.targetposition)
self.system.setmeasures(self.measures)
self.currentposition = self.system.getpositionusingrssiranging(
self.references[0], self.references[1],
(self.currentposition.getx() + 1, self.currentposition.gety() + 1))
self.trajectory.append(self.currentposition)
if self.canvas and len(self.measures) > 2:
self.canvas.paint(self.currentposition.getx(),
self.currentposition.gety())
self.pointscondition.acquire()
points_monitor.enqueuepoint(
Position(x=self.currentposition.getx(),
y=self.currentposition.gety()))
self.pointscondition.notify()
self.pointscondition.release()
self.feedbackcondition.acquire()
feedback_monitor.enqueuepoint(
Position(x=self.currentposition.getx(),
y=self.currentposition.gety()))
self.feedbackcondition.notify()
self.feedbackcondition.release()
return True
def isnewdata(self):
"""This method ask if there is new measures available for triggering a new location process"""
self.condition.acquire()
newdata = measures_monitor.isempty()
self.condition.notify()
self.condition.release()
return not newdata
def getnewdata(self):
"""This method ask the new measures available for location process"""
flag = False
self.condition.acquire()
while True:
measure = measures_monitor.dequeuemeasure()
if measure:
if len(self.measures) <= MAX_MEASURES:
self.measures.append(measure)
else:
self.measures.pop(0)
self.measures.append(measure)
flag = True
break
self.condition.wait()
self.condition.notify()
self.condition.release()
print(len(self.measures))
return flag
def isarrived(self, precision=1):
"""This method checks if the current position is near enough to finish position"""
projectedcurrentposition = self.projecttoground(self.currentposition)
projectedtargetposition = self.projecttoground(self.targetposition)
print("projected", projectedtargetposition, projectedcurrentposition)
if projectedcurrentposition and projectedtargetposition:
distance = sqrt(
pow(
projectedtargetposition.getx() -
projectedcurrentposition.getx(), 2) + pow(
projectedtargetposition.gety() -
projectedcurrentposition.gety(), 2))
print(distance)
if distance < precision and self.state == "LOCATED" and len(
self.measures) > 10:
return True
else:
return False
else:
return False
"""miso_beacon_radiodet_gonio"""
from miso_beacon_radiodet.position import Position
from miso_beacon_radiodet.probe import Probe
from miso_beacon_radiodet.measure import Measure
from miso_beacon_radiodet.miso_beacon_radiodet_gonio.radiogoniometer import Radiogoniometer
punto1 = Position(x=0, y=0, z=0)
punto2 = Position(x=1, y=0, z=0)
probe1 = Probe(punto1)
probe2 = Probe(punto2)
measure1 = Measure(1, 0)
measure2 = Measure(2, 3.33564095e-9)
probe1.addmeasure(measure1)
probe2.addmeasure(measure2)
probes = [probe1, probe2]
radiogoniometro = Radiogoniometer(probes)
angle = radiogoniometro.gettimelapseangle()
if angle == 1.5707618581188734:
print("miso_beacon_radiodet_gonio: SUCCESS")
else:
print("miso_beacon_radiodet_gonio: FAILED")
def setinitialposition(self, position=Position()):
"""Initial position setter"""
self.initialposition = position