def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.automataGui.show()
app.exec_()
def runGui(self): app = QtGui.QApplication(sys.argv) self.automataGui = AutomataGui() self.automataGui.setAutomata(self.createAutomata()) self.automataGui.loadAutomata() self.automataGui.show() app.exec_()
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"state1",
"state2",
]
self.StatesSub2 = [
"state11",
"state11_ghost",
"state12",
"state12_ghost",
]
self.sub1 = "state1"
self.run1 = True
self.sub2 = "state11_ghost"
self.run2 = True
int functions(int a, int b) {
return a+b;
}
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t2 = threading.Thread(target=self.subautomata2)
self.t2.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 2, 118, 114, 1, 'state1')
guiSubautomata1.newGuiNode(2, 0, 354, 313, 0, 'state2')
guiSubautomata1.newGuiTransition((118, 114), (354, 313), (299, 156), 1, 1, 2)
guiSubautomata1.newGuiTransition((354, 313), (118, 114), (171, 304), 3, 2, 1)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata2
guiSubautomata2 = GuiSubautomata(2,1, self.automataGui)
guiSubautomata2.newGuiNode(3, 0, 147, 123, 1, 'state11')
guiSubautomata2.newGuiNode(4, 0, 407, 347, 0, 'state12')
guiSubautomata2.newGuiTransition((147, 123), (407, 347), (277, 235), 2, 3, 4)
guiSubautomataList.append(guiSubautomata2)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run2 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 200
t_activated = False
t_fin = 0
int variables = 10;
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "state1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3):
self.sub1 = "state2"
t_activated = False
int transition_code = 12;
if self.displayGui:
self.automataGui.notifySetNodeAsActive('state2')
elif(self.sub1 == "state2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3):
self.sub1 = "state1"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('state1')
# Actuation if
if(self.sub1 == "state1"):
int state1_code = 10;
elif(self.sub1 == "state2"):
int state2_code = 10;
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata2(self):
self.run2 = True
cycle = 100
t_activated = False
t_fin = 0
t_state11_max = 10
while(self.run2):
totala = time.time() * 1000000
if(self.sub1 == "state1"):
if ((self.sub2 == "state11_ghost") or (self.sub2 == "state12_ghost")):
ghostStateIndex = self.StatesSub2.index(self.sub2)
self.sub2 = self.StatesSub2[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub2 == "state11"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_state11_max):
self.sub2 = "state12"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('state12')
t_state11_max = 10
# Actuation if
if(self.sub2 == "state11"):
myMotors->setV(0.2);
elif(self.sub2 == "state12"):
myMotors->setV(0.0);
else:
if(self.sub2 == "state11"):
t_state11_max = 10 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
elif(self.sub2 == "state12"):
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = EasyIce.initialize(sys.argv)
# Contact to myMotors
myMotors = self.ic.propertyToProxy('automata.myMotors.Proxy')
if(not myMotors):
raise Exception('could not create proxy with myMotors')
self.myMotors = MotorsPrx.checkedCast(myMotors)
if(not self.myMotors):
raise Exception('invalid proxy automata.myMotors.Proxy')
print('myMotors connected')
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t2.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print('runtime gui enabled')
else:
self.displayGui = False
print('runtime gui disabled')
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"GoFront",
"DoSquare",
"Landing",
"END",
]
self.StatesSub2 = [
"TakingOff",
"TakingOff_ghost",
"Stabilizing1",
"Stabilizing1_ghost",
]
self.StatesSub3 = [
"GoingFront",
"GoingFront_ghost",
"Stabilizing2",
"Stabilizing2_ghost",
"goneFront",
"goneFront_ghost",
]
self.StatesSub4 = [
"empty1",
"empty1_ghost",
"empty2",
"empty2_ghost",
]
self.StatesSub5 = [
"DoingRigthSide",
"DoingRigthSide_ghost",
"DoingTopSide",
"DoingTopSide_ghost",
"DoingLeftSide",
"DoingLeftSide_ghost",
"DoingBottomSide",
"DoingBottomSide_ghost",
]
self.StatesSub6 = [
"GoingToTop",
"GoingToTop_ghost",
"StabilizingTop",
"StabilizingTop_ghost",
"GoneToTop",
"GoneToTop_ghost",
]
self.StatesSub8 = [
"GoingToLeft",
"GoingToLeft_ghost",
"StabilizingLeft",
"StabilizingLeft_ghost",
"GoneToLeft",
"GoneToLeft_ghost",
]
self.StatesSub9 = [
"GoingToBottom",
"GoingToBottom_ghost",
"StabilizingBottom",
"StabilizingBottom_ghost",
"GoneToBottom",
"GoneToBottom_ghost",
]
self.StatesSub10 = [
"GoingToRigth",
"GoingToRigth_ghost",
"StabilizingRigth",
"StabilizingRigth_ghost",
"GoneToRigth",
"GoneToRigth_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub2 = "TakingOff_ghost"
self.run2 = True
self.sub3 = "GoingFront_ghost"
self.run3 = True
self.sub4 = "empty1_ghost"
self.run4 = True
self.sub5 = "DoingRigthSide_ghost"
self.run5 = True
self.sub6 = "GoingToTop_ghost"
self.run6 = True
self.sub8 = "GoingToLeft_ghost"
self.run8 = True
self.sub9 = "GoingToBottom_ghost"
self.run9 = True
self.sub10 = "GoingToRigth_ghost"
self.run10 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t2 = threading.Thread(target=self.subautomata2)
self.t2.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t4 = threading.Thread(target=self.subautomata4)
self.t4.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
self.t6 = threading.Thread(target=self.subautomata6)
self.t6.start()
self.t8 = threading.Thread(target=self.subautomata8)
self.t8.start()
self.t9 = threading.Thread(target=self.subautomata9)
self.t9.start()
self.t10 = threading.Thread(target=self.subautomata10)
self.t10.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 2, 76, 158, 1, 'TakeOff')
guiSubautomata1.newGuiNode(5, 3, 323, 136, 0, 'GoFront')
guiSubautomata1.newGuiNode(14, 5, 587, 181, 0, 'DoSquare')
guiSubautomata1.newGuiNode(15, 0, 395, 467, 0, 'Landing')
guiSubautomata1.newGuiNode(16, 0, 89, 421, 0, 'END')
guiSubautomata1.newGuiTransition((76, 158), (323, 136), (189, 145), 2, 1, 5)
guiSubautomata1.newGuiTransition((323, 136), (587, 181), (446, 132), 6, 5, 14)
guiSubautomata1.newGuiTransition((395, 467), (89, 421), (226, 464), 8, 15, 16)
guiSubautomata1.newGuiTransition((89, 421), (76, 158), (48, 290), 28, 16, 1)
guiSubautomata1.newGuiTransition((587, 181), (395, 467), (491, 324), 37, 14, 15)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata2
guiSubautomata2 = GuiSubautomata(2,1, self.automataGui)
guiSubautomata2.newGuiNode(2, 0, 106, 141, 1, 'TakingOff')
guiSubautomata2.newGuiNode(3, 0, 283, 219, 0, 'Stabilizing1')
guiSubautomata2.newGuiTransition((106, 141), (283, 219), (138, 243), 1, 2, 3)
guiSubautomata2.newGuiTransition((283, 219), (106, 141), (244, 116), 26, 3, 2)
guiSubautomataList.append(guiSubautomata2)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3,5, self.automataGui)
guiSubautomata3.newGuiNode(9, 0, 127, 107, 1, 'GoingFront')
guiSubautomata3.newGuiNode(10, 4, 511, 137, 0, 'Stabilizing2')
guiSubautomata3.newGuiNode(11, 0, 298, 291, 0, 'goneFront')
guiSubautomata3.newGuiTransition((127, 107), (511, 137), (319, 123), 2, 9, 10)
guiSubautomata3.newGuiTransition((511, 137), (298, 291), (405, 214), 3, 10, 11)
guiSubautomata3.newGuiTransition((298, 291), (127, 107), (212, 199), 27, 11, 9)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata4
guiSubautomata4 = GuiSubautomata(4,10, self.automataGui)
guiSubautomata4.newGuiNode(12, 0, 232, 178, 1, 'empty1')
guiSubautomata4.newGuiNode(13, 0, 445, 217, 0, 'empty2')
guiSubautomata4.newGuiTransition((232, 178), (445, 217), (357, 129), 4, 12, 13)
guiSubautomata4.newGuiTransition((445, 217), (232, 178), (316, 271), 5, 13, 12)
guiSubautomataList.append(guiSubautomata4)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5,14, self.automataGui)
guiSubautomata5.newGuiNode(17, 6, 465, 388, 1, 'DoingRigthSide')
guiSubautomata5.newGuiNode(18, 8, 461, 140, 0, 'DoingTopSide')
guiSubautomata5.newGuiNode(19, 9, 134, 130, 0, 'DoingLeftSide')
guiSubautomata5.newGuiNode(20, 10, 141, 389, 0, 'DoingBottomSide')
guiSubautomata5.newGuiTransition((465, 388), (461, 140), (463, 264), 9, 17, 18)
guiSubautomata5.newGuiTransition((461, 140), (134, 130), (298, 136), 10, 18, 19)
guiSubautomata5.newGuiTransition((134, 130), (141, 389), (137, 260), 11, 19, 20)
guiSubautomata5.newGuiTransition((141, 389), (465, 388), (303, 388), 12, 20, 17)
guiSubautomataList.append(guiSubautomata5)
# Creating subAutomata6
guiSubautomata6 = GuiSubautomata(6,17, self.automataGui)
guiSubautomata6.newGuiNode(21, 0, 352, 452, 1, 'GoingToTop')
guiSubautomata6.newGuiNode(22, 0, 280, 218, 0, 'StabilizingTop')
guiSubautomata6.newGuiNode(23, 0, 448, 105, 0, 'GoneToTop')
guiSubautomata6.newGuiTransition((352, 452), (280, 218), (212, 319), 13, 21, 22)
guiSubautomata6.newGuiTransition((280, 218), (448, 105), (350, 132), 14, 22, 23)
guiSubautomata6.newGuiTransition((448, 105), (352, 452), (400, 279), 16, 23, 21)
guiSubautomataList.append(guiSubautomata6)
# Creating subAutomata8
guiSubautomata8 = GuiSubautomata(8,18, self.automataGui)
guiSubautomata8.newGuiNode(24, 0, 135, 397, 1, 'GoingToLeft')
guiSubautomata8.newGuiNode(25, 0, 213, 194, 0, 'StabilizingLeft')
guiSubautomata8.newGuiNode(26, 0, 416, 260, 0, 'GoneToLeft')
guiSubautomata8.newGuiTransition((135, 397), (213, 194), (174, 295), 17, 24, 25)
guiSubautomata8.newGuiTransition((213, 194), (416, 260), (314, 227), 18, 25, 26)
guiSubautomata8.newGuiTransition((416, 260), (135, 397), (275, 328), 19, 26, 24)
guiSubautomataList.append(guiSubautomata8)
# Creating subAutomata9
guiSubautomata9 = GuiSubautomata(9,19, self.automataGui)
guiSubautomata9.newGuiNode(27, 0, 169, 408, 1, 'GoingToBottom')
guiSubautomata9.newGuiNode(28, 0, 243, 164, 0, 'StabilizingBottom')
guiSubautomata9.newGuiNode(29, 0, 418, 355, 0, 'GoneToBottom')
guiSubautomata9.newGuiTransition((169, 408), (243, 164), (206, 286), 20, 27, 28)
guiSubautomata9.newGuiTransition((243, 164), (418, 355), (330, 259), 21, 28, 29)
guiSubautomata9.newGuiTransition((418, 355), (169, 408), (293, 381), 22, 29, 27)
guiSubautomataList.append(guiSubautomata9)
# Creating subAutomata10
guiSubautomata10 = GuiSubautomata(10,20, self.automataGui)
guiSubautomata10.newGuiNode(31, 0, 121, 361, 1, 'GoingToRigth')
guiSubautomata10.newGuiNode(32, 0, 231, 110, 0, 'StabilizingRigth')
guiSubautomata10.newGuiNode(33, 0, 371, 331, 0, 'GoneToRigth')
guiSubautomata10.newGuiTransition((121, 361), (231, 110), (176, 235), 23, 31, 32)
guiSubautomata10.newGuiTransition((231, 110), (371, 331), (300, 220), 24, 32, 33)
guiSubautomata10.newGuiTransition((371, 331), (121, 361), (246, 346), 25, 33, 31)
guiSubautomataList.append(guiSubautomata10)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run2 = False
self.run3 = False
self.run4 = False
self.run5 = False
self.run6 = False
self.run8 = False
self.run9 = False
self.run10 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
self.goneFront = False
self.squareDone = False
self.goneBack = False
while(run):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "TakeOff"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3.5):
self.sub1 = "GoFront"
t_activated = False
print "From TakeOff to GoFront"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoFront')
elif(self.sub1 == "GoFront"):
if(self.goneFront):
self.sub1 = "DoSquare"
self.goneFront = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('DoSquare')
elif(self.sub1 == "DoSquare"):
if(self.squareDone):
self.sub1 = "Landing"
self.squareDone = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Landing')
elif(self.sub1 == "Landing"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 2):
self.sub1 = "END"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('END')
elif(self.sub1 == "END"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3):
self.sub1 = "TakeOff"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('TakeOff')
# Actuation if
if(self.sub1 == "Landing"):
print "landing"
self.lock.acquire()
self.extraPrx.land()
self.lock.release()
elif(self.sub1 == "END"):
print "END"
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata2(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_TakingOff_max = 2
t_Stabilizing1_max = 1.7
while(run):
totala = time.time() * 1000000
if(self.sub1 == "TakeOff"):
if ((self.sub2 == "TakingOff_ghost") or (self.sub2 == "Stabilizing1_ghost")):
ghostStateIndex = self.StatesSub2.index(self.sub2)
self.sub2 = self.StatesSub2[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub2 == "TakingOff"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_TakingOff_max):
self.sub2 = "Stabilizing1"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Stabilizing1')
t_TakingOff_max = 2
elif(self.sub2 == "Stabilizing1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_Stabilizing1_max):
self.sub2 = "TakingOff"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('TakingOff')
t_Stabilizing1_max = 1.7
# Actuation if
if(self.sub2 == "TakingOff"):
print "Taking Off"
self.lock.acquire()
self.extraPrx.takeoff()
self.lock.release()
else:
if(self.sub2 == "TakingOff"):
t_TakingOff_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
elif(self.sub2 == "Stabilizing1"):
t_Stabilizing1_max = 1.7 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata3(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_Stabilizing2_max = 3
t_goneFront_max = 0.2
initPos = 0
pos = 0
while(run):
totala = time.time() * 1000000
if(self.sub1 == "GoFront"):
if ((self.sub3 == "GoingFront_ghost") or (self.sub3 == "Stabilizing2_ghost") or (self.sub3 == "goneFront_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub3 == "GoingFront"):
if(pos - initPos > 300):
self.sub3 = "Stabilizing2"
print "300m reached"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Stabilizing2')
elif(self.sub3 == "Stabilizing2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_Stabilizing2_max):
self.sub3 = "goneFront"
t_activated = False
print "from stabilizing2 to GoneFront"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('goneFront')
t_Stabilizing2_max = 3
elif(self.sub3 == "goneFront"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_goneFront_max):
self.sub3 = "GoingFront"
t_activated = False
initPos = 0
pos = 0
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoingFront')
t_goneFront_max = 0.2
# Actuation if
if(self.sub3 == "GoingFront"):
pos = self.pose3dPrx.getPose3DData().x * 100
if initPos == 0:
initPos = self.pose3dPrx.getPose3DData().x * 100
print "pos:", pos, "init:", initPos
print "distancia", pos - initPos
cmd = jderobot.CMDVelData()
cmd.linearX = 1
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub3 == "Stabilizing2"):
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub3 == "goneFront"):
self.goneFront = True
else:
if(self.sub3 == "GoingFront"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "Stabilizing2"):
t_Stabilizing2_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "goneFront"):
t_goneFront_max = 0.2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata4(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_empty1_max = 0.8
t_empty2_max = 0.8
while(run):
totala = time.time() * 1000000
if(self.sub3 == "Stabilizing2"):
if ((self.sub4 == "empty1_ghost") or (self.sub4 == "empty2_ghost")):
ghostStateIndex = self.StatesSub4.index(self.sub4)
self.sub4 = self.StatesSub4[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub4 == "empty1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_empty1_max):
self.sub4 = "empty2"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('empty2')
t_empty1_max = 0.8
elif(self.sub4 == "empty2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_empty2_max):
self.sub4 = "empty1"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('empty1')
t_empty2_max = 0.8
# Actuation if
else:
if(self.sub4 == "empty1"):
t_empty1_max = 0.8 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif(self.sub4 == "empty2"):
t_empty2_max = 0.8 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata5(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
self.goneToTop = False
self.goneToLeft = False
self.goneToBottom = False
self.goneToRigth = False
while(run):
totala = time.time() * 1000000
if(self.sub1 == "DoSquare"):
if ((self.sub5 == "DoingRigthSide_ghost") or (self.sub5 == "DoingTopSide_ghost") or (self.sub5 == "DoingLeftSide_ghost") or (self.sub5 == "DoingBottomSide_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub5 == "DoingRigthSide"):
if(self.goneToTop):
self.sub5 = "DoingTopSide"
print "Rigth Done"
self.goneToTop = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('DoingTopSide')
elif(self.sub5 == "DoingTopSide"):
if(self.goneToLeft):
self.sub5 = "DoingLeftSide"
print "Left done"
self.goneToLeft = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('DoingLeftSide')
elif(self.sub5 == "DoingLeftSide"):
if(self.goneToBottom):
self.sub5 = "DoingBottomSide"
print "Left done"
self.goneToBottom = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('DoingBottomSide')
elif(self.sub5 == "DoingBottomSide"):
if(self.goneToRigth):
self.sub5 = "DoingRigthSide"
self.squareDone = True
print "Bottom done"
self.goneToRigth = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('DoingRigthSide')
# Actuation if
if(self.sub5 == "DoingRigthSide"):
print "Doing Rigth Side"
elif(self.sub5 == "DoingTopSide"):
print "Doing Top Side"
elif(self.sub5 == "DoingLeftSide"):
print "Doing Left Side"
elif(self.sub5 == "DoingBottomSide"):
print "Doing Bottom Side"
else:
if(self.sub5 == "DoingRigthSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "DoingTopSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "DoingLeftSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "DoingBottomSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata6(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToTop_max = 3
t_StabilizingTop_max = 2
t_GoneToTop_max = 0.1
while(run):
totala = time.time() * 1000000
if(self.sub5 == "DoingRigthSide"):
if ((self.sub6 == "GoingToTop_ghost") or (self.sub6 == "StabilizingTop_ghost") or (self.sub6 == "GoneToTop_ghost")):
ghostStateIndex = self.StatesSub6.index(self.sub6)
self.sub6 = self.StatesSub6[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub6 == "GoingToTop"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoingToTop_max):
self.sub6 = "StabilizingTop"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('StabilizingTop')
t_GoingToTop_max = 3
elif(self.sub6 == "StabilizingTop"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_StabilizingTop_max):
self.sub6 = "GoneToTop"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoneToTop')
t_StabilizingTop_max = 2
elif(self.sub6 == "GoneToTop"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoneToTop_max):
self.sub6 = "GoingToTop"
t_activated = False
self.GoneToTop = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoingToTop')
t_GoneToTop_max = 0.1
# Actuation if
if(self.sub6 == "GoingToTop"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0.75
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub6 == "StabilizingTop"):
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub6 == "GoneToTop"):
self.goneToTop = True
else:
if(self.sub6 == "GoingToTop"):
t_GoingToTop_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif(self.sub6 == "StabilizingTop"):
t_StabilizingTop_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif(self.sub6 == "GoneToTop"):
t_GoneToTop_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata8(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToLeft_max = 3
t_StabilizingLeft_max = 2
t_GoneToLeft_max = 0.1
while(run):
totala = time.time() * 1000000
if(self.sub5 == "DoingTopSide"):
if ((self.sub8 == "GoingToLeft_ghost") or (self.sub8 == "StabilizingLeft_ghost") or (self.sub8 == "GoneToLeft_ghost")):
ghostStateIndex = self.StatesSub8.index(self.sub8)
self.sub8 = self.StatesSub8[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub8 == "GoingToLeft"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoingToLeft_max):
self.sub8 = "StabilizingLeft"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('StabilizingLeft')
t_GoingToLeft_max = 3
elif(self.sub8 == "StabilizingLeft"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_StabilizingLeft_max):
self.sub8 = "GoneToLeft"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoneToLeft')
t_StabilizingLeft_max = 2
elif(self.sub8 == "GoneToLeft"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoneToLeft_max):
self.sub8 = "GoingToLeft"
t_activated = False
self.goneToLeft = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoingToLeft')
t_GoneToLeft_max = 0.1
# Actuation if
if(self.sub8 == "GoingToLeft"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0.75
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub8 == "StabilizingLeft"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub8 == "GoneToLeft"):
self.goneToLeft = True
else:
if(self.sub8 == "GoingToLeft"):
t_GoingToLeft_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
self.sub8 = self.StatesSub8[ghostStateIndex]
elif(self.sub8 == "StabilizingLeft"):
t_StabilizingLeft_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
self.sub8 = self.StatesSub8[ghostStateIndex]
elif(self.sub8 == "GoneToLeft"):
t_GoneToLeft_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
self.sub8 = self.StatesSub8[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata9(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToBottom_max = 3
t_StabilizingBottom_max = 2
t_GoneToBottom_max = 0.1
while(run):
totala = time.time() * 1000000
if(self.sub5 == "DoingLeftSide"):
if ((self.sub9 == "GoingToBottom_ghost") or (self.sub9 == "StabilizingBottom_ghost") or (self.sub9 == "GoneToBottom_ghost")):
ghostStateIndex = self.StatesSub9.index(self.sub9)
self.sub9 = self.StatesSub9[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub9 == "GoingToBottom"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoingToBottom_max):
self.sub9 = "StabilizingBottom"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('StabilizingBottom')
t_GoingToBottom_max = 3
elif(self.sub9 == "StabilizingBottom"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_StabilizingBottom_max):
self.sub9 = "GoneToBottom"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoneToBottom')
t_StabilizingBottom_max = 2
elif(self.sub9 == "GoneToBottom"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoneToBottom_max):
self.sub9 = "GoingToBottom"
t_activated = False
self.goneToBottom = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoingToBottom')
t_GoneToBottom_max = 0.1
# Actuation if
if(self.sub9 == "GoingToBottom"):
cmd = jderobot.CMDVelData()
cmd.linearX = -0.75
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub9 == "StabilizingBottom"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub9 == "GoneToBottom"):
self.goneToBottom = True
else:
if(self.sub9 == "GoingToBottom"):
t_GoingToBottom_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
self.sub9 = self.StatesSub9[ghostStateIndex]
elif(self.sub9 == "StabilizingBottom"):
t_StabilizingBottom_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
self.sub9 = self.StatesSub9[ghostStateIndex]
elif(self.sub9 == "GoneToBottom"):
t_GoneToBottom_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
self.sub9 = self.StatesSub9[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata10(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToRigth_max = 3
t_StabilizingRigth_max = 2
t_GoneToRigth_max = 0.1
while(run):
totala = time.time() * 1000000
if(self.sub5 == "DoingBottomSide"):
if ((self.sub10 == "GoingToRigth_ghost") or (self.sub10 == "StabilizingRigth_ghost") or (self.sub10 == "GoneToRigth_ghost")):
ghostStateIndex = self.StatesSub10.index(self.sub10)
self.sub10 = self.StatesSub10[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub10 == "GoingToRigth"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoingToRigth_max):
self.sub10 = "StabilizingRigth"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('StabilizingRigth')
t_GoingToRigth_max = 3
elif(self.sub10 == "StabilizingRigth"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_StabilizingRigth_max):
self.sub10 = "GoneToRigth"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoneToRigth')
t_StabilizingRigth_max = 2
elif(self.sub10 == "GoneToRigth"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoneToRigth_max):
self.sub10 = "GoingToRigth"
t_activated = False
self.goneToRigth = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoingToRigth')
t_GoneToRigth_max = 0.1
# Actuation if
if(self.sub10 == "GoingToRigth"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = -0.75
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub10 == "StabilizingRigth"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub10 == "GoneToRigth"):
self.goneToRigth = True
else:
if(self.sub10 == "GoingToRigth"):
t_GoingToRigth_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
self.sub10 = self.StatesSub10[ghostStateIndex]
elif(self.sub10 == "StabilizingRigth"):
t_StabilizingRigth_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
self.sub10 = self.StatesSub10[ghostStateIndex]
elif(self.sub10 == "GoneToRigth"):
t_GoneToRigth_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
self.sub10 = self.StatesSub10[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to pose3d
pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if(not pose3d):
raise Exception('could not create proxy with pose3d')
self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
if(not self.pose3dPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'pose3d connected'
# Contact to cmdVel
cmdVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if(not cmdVel):
raise Exception('could not create proxy with cmdVel')
self.cmdVelPrx = CMDVelPrx.checkedCast(cmdVel)
if(not self.cmdVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'cmdVel connected'
# Contact to extra
extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if(not extra):
raise Exception('could not create proxy with extra')
self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
if(not self.extraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'extra connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t2.join()
self.t3.join()
self.t4.join()
self.t5.join()
self.t6.join()
self.t8.join()
self.t9.join()
self.t10.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"GoFront",
"DoSquare",
"Landing",
"END",
]
self.StatesSub2 = [
"TakingOff",
"TakingOff_ghost",
"Stabilizing1",
"Stabilizing1_ghost",
]
self.StatesSub3 = [
"GoingFront",
"GoingFront_ghost",
"Stabilizing2",
"Stabilizing2_ghost",
"goneFront",
"goneFront_ghost",
]
self.StatesSub4 = [
"empty1",
"empty1_ghost",
"empty2",
"empty2_ghost",
]
self.StatesSub5 = [
"DoingRigthSide",
"DoingRigthSide_ghost",
"DoingTopSide",
"DoingTopSide_ghost",
"DoingLeftSide",
"DoingLeftSide_ghost",
"DoingBottomSide",
"DoingBottomSide_ghost",
]
self.StatesSub6 = [
"GoingToTop",
"GoingToTop_ghost",
"StabilizingTop",
"StabilizingTop_ghost",
"GoneToTop",
"GoneToTop_ghost",
]
self.StatesSub8 = [
"GoingToLeft",
"GoingToLeft_ghost",
"StabilizingLeft",
"StabilizingLeft_ghost",
"GoneToLeft",
"GoneToLeft_ghost",
]
self.StatesSub9 = [
"GoingToBottom",
"GoingToBottom_ghost",
"StabilizingBottom",
"StabilizingBottom_ghost",
"GoneToBottom",
"GoneToBottom_ghost",
]
self.StatesSub10 = [
"GoingToRigth",
"GoingToRigth_ghost",
"StabilizingRigth",
"StabilizingRigth_ghost",
"GoneToRigth",
"GoneToRigth_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub2 = "TakingOff_ghost"
self.run2 = True
self.sub3 = "GoingFront_ghost"
self.run3 = True
self.sub4 = "empty1_ghost"
self.run4 = True
self.sub5 = "DoingRigthSide_ghost"
self.run5 = True
self.sub6 = "GoingToTop_ghost"
self.run6 = True
self.sub8 = "GoingToLeft_ghost"
self.run8 = True
self.sub9 = "GoingToBottom_ghost"
self.run9 = True
self.sub10 = "GoingToRigth_ghost"
self.run10 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t2 = threading.Thread(target=self.subautomata2)
self.t2.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t4 = threading.Thread(target=self.subautomata4)
self.t4.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
self.t6 = threading.Thread(target=self.subautomata6)
self.t6.start()
self.t8 = threading.Thread(target=self.subautomata8)
self.t8.start()
self.t9 = threading.Thread(target=self.subautomata9)
self.t9.start()
self.t10 = threading.Thread(target=self.subautomata10)
self.t10.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 2, 76, 158, 1, 'TakeOff')
guiSubautomata1.newGuiNode(5, 3, 323, 136, 0, 'GoFront')
guiSubautomata1.newGuiNode(14, 5, 587, 181, 0, 'DoSquare')
guiSubautomata1.newGuiNode(15, 0, 395, 467, 0, 'Landing')
guiSubautomata1.newGuiNode(16, 0, 89, 421, 0, 'END')
guiSubautomata1.newGuiTransition((76, 158), (323, 136), (189, 145), 2,
1, 5)
guiSubautomata1.newGuiTransition((323, 136), (587, 181), (446, 132), 6,
5, 14)
guiSubautomata1.newGuiTransition((395, 467), (89, 421), (226, 464), 8,
15, 16)
guiSubautomata1.newGuiTransition((89, 421), (76, 158), (48, 290), 28,
16, 1)
guiSubautomata1.newGuiTransition((587, 181), (395, 467), (491, 324),
37, 14, 15)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata2
guiSubautomata2 = GuiSubautomata(2, 1, self.automataGui)
guiSubautomata2.newGuiNode(2, 0, 106, 141, 1, 'TakingOff')
guiSubautomata2.newGuiNode(3, 0, 283, 219, 0, 'Stabilizing1')
guiSubautomata2.newGuiTransition((106, 141), (283, 219), (138, 243), 1,
2, 3)
guiSubautomata2.newGuiTransition((283, 219), (106, 141), (244, 116),
26, 3, 2)
guiSubautomataList.append(guiSubautomata2)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3, 5, self.automataGui)
guiSubautomata3.newGuiNode(9, 0, 127, 107, 1, 'GoingFront')
guiSubautomata3.newGuiNode(10, 4, 511, 137, 0, 'Stabilizing2')
guiSubautomata3.newGuiNode(11, 0, 298, 291, 0, 'goneFront')
guiSubautomata3.newGuiTransition((127, 107), (511, 137), (319, 123), 2,
9, 10)
guiSubautomata3.newGuiTransition((511, 137), (298, 291), (405, 214), 3,
10, 11)
guiSubautomata3.newGuiTransition((298, 291), (127, 107), (212, 199),
27, 11, 9)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata4
guiSubautomata4 = GuiSubautomata(4, 10, self.automataGui)
guiSubautomata4.newGuiNode(12, 0, 232, 178, 1, 'empty1')
guiSubautomata4.newGuiNode(13, 0, 445, 217, 0, 'empty2')
guiSubautomata4.newGuiTransition((232, 178), (445, 217), (357, 129), 4,
12, 13)
guiSubautomata4.newGuiTransition((445, 217), (232, 178), (316, 271), 5,
13, 12)
guiSubautomataList.append(guiSubautomata4)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5, 14, self.automataGui)
guiSubautomata5.newGuiNode(17, 6, 465, 388, 1, 'DoingRigthSide')
guiSubautomata5.newGuiNode(18, 8, 461, 140, 0, 'DoingTopSide')
guiSubautomata5.newGuiNode(19, 9, 134, 130, 0, 'DoingLeftSide')
guiSubautomata5.newGuiNode(20, 10, 141, 389, 0, 'DoingBottomSide')
guiSubautomata5.newGuiTransition((465, 388), (461, 140), (463, 264), 9,
17, 18)
guiSubautomata5.newGuiTransition((461, 140), (134, 130), (298, 136),
10, 18, 19)
guiSubautomata5.newGuiTransition((134, 130), (141, 389), (137, 260),
11, 19, 20)
guiSubautomata5.newGuiTransition((141, 389), (465, 388), (303, 388),
12, 20, 17)
guiSubautomataList.append(guiSubautomata5)
# Creating subAutomata6
guiSubautomata6 = GuiSubautomata(6, 17, self.automataGui)
guiSubautomata6.newGuiNode(21, 0, 352, 452, 1, 'GoingToTop')
guiSubautomata6.newGuiNode(22, 0, 280, 218, 0, 'StabilizingTop')
guiSubautomata6.newGuiNode(23, 0, 448, 105, 0, 'GoneToTop')
guiSubautomata6.newGuiTransition((352, 452), (280, 218), (212, 319),
13, 21, 22)
guiSubautomata6.newGuiTransition((280, 218), (448, 105), (350, 132),
14, 22, 23)
guiSubautomata6.newGuiTransition((448, 105), (352, 452), (400, 279),
16, 23, 21)
guiSubautomataList.append(guiSubautomata6)
# Creating subAutomata8
guiSubautomata8 = GuiSubautomata(8, 18, self.automataGui)
guiSubautomata8.newGuiNode(24, 0, 135, 397, 1, 'GoingToLeft')
guiSubautomata8.newGuiNode(25, 0, 213, 194, 0, 'StabilizingLeft')
guiSubautomata8.newGuiNode(26, 0, 416, 260, 0, 'GoneToLeft')
guiSubautomata8.newGuiTransition((135, 397), (213, 194), (174, 295),
17, 24, 25)
guiSubautomata8.newGuiTransition((213, 194), (416, 260), (314, 227),
18, 25, 26)
guiSubautomata8.newGuiTransition((416, 260), (135, 397), (275, 328),
19, 26, 24)
guiSubautomataList.append(guiSubautomata8)
# Creating subAutomata9
guiSubautomata9 = GuiSubautomata(9, 19, self.automataGui)
guiSubautomata9.newGuiNode(27, 0, 169, 408, 1, 'GoingToBottom')
guiSubautomata9.newGuiNode(28, 0, 243, 164, 0, 'StabilizingBottom')
guiSubautomata9.newGuiNode(29, 0, 418, 355, 0, 'GoneToBottom')
guiSubautomata9.newGuiTransition((169, 408), (243, 164), (206, 286),
20, 27, 28)
guiSubautomata9.newGuiTransition((243, 164), (418, 355), (330, 259),
21, 28, 29)
guiSubautomata9.newGuiTransition((418, 355), (169, 408), (293, 381),
22, 29, 27)
guiSubautomataList.append(guiSubautomata9)
# Creating subAutomata10
guiSubautomata10 = GuiSubautomata(10, 20, self.automataGui)
guiSubautomata10.newGuiNode(31, 0, 121, 361, 1, 'GoingToRigth')
guiSubautomata10.newGuiNode(32, 0, 231, 110, 0, 'StabilizingRigth')
guiSubautomata10.newGuiNode(33, 0, 371, 331, 0, 'GoneToRigth')
guiSubautomata10.newGuiTransition((121, 361), (231, 110), (176, 235),
23, 31, 32)
guiSubautomata10.newGuiTransition((231, 110), (371, 331), (300, 220),
24, 32, 33)
guiSubautomata10.newGuiTransition((371, 331), (121, 361), (246, 346),
25, 33, 31)
guiSubautomataList.append(guiSubautomata10)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run2 = False
self.run3 = False
self.run4 = False
self.run5 = False
self.run6 = False
self.run8 = False
self.run9 = False
self.run10 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
self.goneFront = False
self.squareDone = False
self.goneBack = False
while (run):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "TakeOff"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 3.5):
self.sub1 = "GoFront"
t_activated = False
print "From TakeOff to GoFront"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoFront')
elif (self.sub1 == "GoFront"):
if (self.goneFront):
self.sub1 = "DoSquare"
self.goneFront = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('DoSquare')
elif (self.sub1 == "DoSquare"):
if (self.squareDone):
self.sub1 = "Landing"
self.squareDone = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Landing')
elif (self.sub1 == "Landing"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 2):
self.sub1 = "END"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('END')
elif (self.sub1 == "END"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 3):
self.sub1 = "TakeOff"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('TakeOff')
# Actuation if
if (self.sub1 == "Landing"):
print "landing"
self.lock.acquire()
self.extraPrx.land()
self.lock.release()
elif (self.sub1 == "END"):
print "END"
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata2(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_TakingOff_max = 2
t_Stabilizing1_max = 1.7
while (run):
totala = time.time() * 1000000
if (self.sub1 == "TakeOff"):
if ((self.sub2 == "TakingOff_ghost")
or (self.sub2 == "Stabilizing1_ghost")):
ghostStateIndex = self.StatesSub2.index(self.sub2)
self.sub2 = self.StatesSub2[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub2 == "TakingOff"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_TakingOff_max):
self.sub2 = "Stabilizing1"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'Stabilizing1')
t_TakingOff_max = 2
elif (self.sub2 == "Stabilizing1"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_Stabilizing1_max):
self.sub2 = "TakingOff"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'TakingOff')
t_Stabilizing1_max = 1.7
# Actuation if
if (self.sub2 == "TakingOff"):
print "Taking Off"
self.lock.acquire()
self.extraPrx.takeoff()
self.lock.release()
else:
if (self.sub2 == "TakingOff"):
t_TakingOff_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
elif (self.sub2 == "Stabilizing1"):
t_Stabilizing1_max = 1.7 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata3(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_Stabilizing2_max = 3
t_goneFront_max = 0.2
initPos = 0
pos = 0
while (run):
totala = time.time() * 1000000
if (self.sub1 == "GoFront"):
if ((self.sub3 == "GoingFront_ghost")
or (self.sub3 == "Stabilizing2_ghost")
or (self.sub3 == "goneFront_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub3 == "GoingFront"):
if (pos - initPos > 300):
self.sub3 = "Stabilizing2"
print "300m reached"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'Stabilizing2')
elif (self.sub3 == "Stabilizing2"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_Stabilizing2_max):
self.sub3 = "goneFront"
t_activated = False
print "from stabilizing2 to GoneFront"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'goneFront')
t_Stabilizing2_max = 3
elif (self.sub3 == "goneFront"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_goneFront_max):
self.sub3 = "GoingFront"
t_activated = False
initPos = 0
pos = 0
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoingFront')
t_goneFront_max = 0.2
# Actuation if
if (self.sub3 == "GoingFront"):
pos = self.pose3dPrx.getPose3DData().x * 100
if initPos == 0:
initPos = self.pose3dPrx.getPose3DData().x * 100
print "pos:", pos, "init:", initPos
print "distancia", pos - initPos
cmd = jderobot.CMDVelData()
cmd.linearX = 1
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub3 == "Stabilizing2"):
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub3 == "goneFront"):
self.goneFront = True
else:
if (self.sub3 == "GoingFront"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif (self.sub3 == "Stabilizing2"):
t_Stabilizing2_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif (self.sub3 == "goneFront"):
t_goneFront_max = 0.2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata4(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_empty1_max = 0.8
t_empty2_max = 0.8
while (run):
totala = time.time() * 1000000
if (self.sub3 == "Stabilizing2"):
if ((self.sub4 == "empty1_ghost")
or (self.sub4 == "empty2_ghost")):
ghostStateIndex = self.StatesSub4.index(self.sub4)
self.sub4 = self.StatesSub4[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub4 == "empty1"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_empty1_max):
self.sub4 = "empty2"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'empty2')
t_empty1_max = 0.8
elif (self.sub4 == "empty2"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_empty2_max):
self.sub4 = "empty1"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'empty1')
t_empty2_max = 0.8
# Actuation if
else:
if (self.sub4 == "empty1"):
t_empty1_max = 0.8 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif (self.sub4 == "empty2"):
t_empty2_max = 0.8 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata5(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
self.goneToTop = False
self.goneToLeft = False
self.goneToBottom = False
self.goneToRigth = False
while (run):
totala = time.time() * 1000000
if (self.sub1 == "DoSquare"):
if ((self.sub5 == "DoingRigthSide_ghost")
or (self.sub5 == "DoingTopSide_ghost")
or (self.sub5 == "DoingLeftSide_ghost")
or (self.sub5 == "DoingBottomSide_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub5 == "DoingRigthSide"):
if (self.goneToTop):
self.sub5 = "DoingTopSide"
print "Rigth Done"
self.goneToTop = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'DoingTopSide')
elif (self.sub5 == "DoingTopSide"):
if (self.goneToLeft):
self.sub5 = "DoingLeftSide"
print "Left done"
self.goneToLeft = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'DoingLeftSide')
elif (self.sub5 == "DoingLeftSide"):
if (self.goneToBottom):
self.sub5 = "DoingBottomSide"
print "Left done"
self.goneToBottom = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'DoingBottomSide')
elif (self.sub5 == "DoingBottomSide"):
if (self.goneToRigth):
self.sub5 = "DoingRigthSide"
self.squareDone = True
print "Bottom done"
self.goneToRigth = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'DoingRigthSide')
# Actuation if
if (self.sub5 == "DoingRigthSide"):
print "Doing Rigth Side"
elif (self.sub5 == "DoingTopSide"):
print "Doing Top Side"
elif (self.sub5 == "DoingLeftSide"):
print "Doing Left Side"
elif (self.sub5 == "DoingBottomSide"):
print "Doing Bottom Side"
else:
if (self.sub5 == "DoingRigthSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "DoingTopSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "DoingLeftSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "DoingBottomSide"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata6(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToTop_max = 3
t_StabilizingTop_max = 2
t_GoneToTop_max = 0.1
while (run):
totala = time.time() * 1000000
if (self.sub5 == "DoingRigthSide"):
if ((self.sub6 == "GoingToTop_ghost")
or (self.sub6 == "StabilizingTop_ghost")
or (self.sub6 == "GoneToTop_ghost")):
ghostStateIndex = self.StatesSub6.index(self.sub6)
self.sub6 = self.StatesSub6[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub6 == "GoingToTop"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoingToTop_max):
self.sub6 = "StabilizingTop"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'StabilizingTop')
t_GoingToTop_max = 3
elif (self.sub6 == "StabilizingTop"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_StabilizingTop_max):
self.sub6 = "GoneToTop"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoneToTop')
t_StabilizingTop_max = 2
elif (self.sub6 == "GoneToTop"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoneToTop_max):
self.sub6 = "GoingToTop"
t_activated = False
self.GoneToTop = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoingToTop')
t_GoneToTop_max = 0.1
# Actuation if
if (self.sub6 == "GoingToTop"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0.75
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub6 == "StabilizingTop"):
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub6 == "GoneToTop"):
self.goneToTop = True
else:
if (self.sub6 == "GoingToTop"):
t_GoingToTop_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif (self.sub6 == "StabilizingTop"):
t_StabilizingTop_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif (self.sub6 == "GoneToTop"):
t_GoneToTop_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata8(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToLeft_max = 3
t_StabilizingLeft_max = 2
t_GoneToLeft_max = 0.1
while (run):
totala = time.time() * 1000000
if (self.sub5 == "DoingTopSide"):
if ((self.sub8 == "GoingToLeft_ghost")
or (self.sub8 == "StabilizingLeft_ghost")
or (self.sub8 == "GoneToLeft_ghost")):
ghostStateIndex = self.StatesSub8.index(self.sub8)
self.sub8 = self.StatesSub8[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub8 == "GoingToLeft"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoingToLeft_max):
self.sub8 = "StabilizingLeft"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'StabilizingLeft')
t_GoingToLeft_max = 3
elif (self.sub8 == "StabilizingLeft"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_StabilizingLeft_max):
self.sub8 = "GoneToLeft"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoneToLeft')
t_StabilizingLeft_max = 2
elif (self.sub8 == "GoneToLeft"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoneToLeft_max):
self.sub8 = "GoingToLeft"
t_activated = False
self.goneToLeft = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoingToLeft')
t_GoneToLeft_max = 0.1
# Actuation if
if (self.sub8 == "GoingToLeft"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0.75
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub8 == "StabilizingLeft"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub8 == "GoneToLeft"):
self.goneToLeft = True
else:
if (self.sub8 == "GoingToLeft"):
t_GoingToLeft_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
self.sub8 = self.StatesSub8[ghostStateIndex]
elif (self.sub8 == "StabilizingLeft"):
t_StabilizingLeft_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
self.sub8 = self.StatesSub8[ghostStateIndex]
elif (self.sub8 == "GoneToLeft"):
t_GoneToLeft_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
self.sub8 = self.StatesSub8[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata9(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToBottom_max = 3
t_StabilizingBottom_max = 2
t_GoneToBottom_max = 0.1
while (run):
totala = time.time() * 1000000
if (self.sub5 == "DoingLeftSide"):
if ((self.sub9 == "GoingToBottom_ghost")
or (self.sub9 == "StabilizingBottom_ghost")
or (self.sub9 == "GoneToBottom_ghost")):
ghostStateIndex = self.StatesSub9.index(self.sub9)
self.sub9 = self.StatesSub9[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub9 == "GoingToBottom"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoingToBottom_max):
self.sub9 = "StabilizingBottom"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'StabilizingBottom')
t_GoingToBottom_max = 3
elif (self.sub9 == "StabilizingBottom"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_StabilizingBottom_max):
self.sub9 = "GoneToBottom"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoneToBottom')
t_StabilizingBottom_max = 2
elif (self.sub9 == "GoneToBottom"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoneToBottom_max):
self.sub9 = "GoingToBottom"
t_activated = False
self.goneToBottom = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoingToBottom')
t_GoneToBottom_max = 0.1
# Actuation if
if (self.sub9 == "GoingToBottom"):
cmd = jderobot.CMDVelData()
cmd.linearX = -0.75
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub9 == "StabilizingBottom"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub9 == "GoneToBottom"):
self.goneToBottom = True
else:
if (self.sub9 == "GoingToBottom"):
t_GoingToBottom_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
self.sub9 = self.StatesSub9[ghostStateIndex]
elif (self.sub9 == "StabilizingBottom"):
t_StabilizingBottom_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
self.sub9 = self.StatesSub9[ghostStateIndex]
elif (self.sub9 == "GoneToBottom"):
t_GoneToBottom_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
self.sub9 = self.StatesSub9[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata10(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_GoingToRigth_max = 3
t_StabilizingRigth_max = 2
t_GoneToRigth_max = 0.1
while (run):
totala = time.time() * 1000000
if (self.sub5 == "DoingBottomSide"):
if ((self.sub10 == "GoingToRigth_ghost")
or (self.sub10 == "StabilizingRigth_ghost")
or (self.sub10 == "GoneToRigth_ghost")):
ghostStateIndex = self.StatesSub10.index(self.sub10)
self.sub10 = self.StatesSub10[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub10 == "GoingToRigth"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoingToRigth_max):
self.sub10 = "StabilizingRigth"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'StabilizingRigth')
t_GoingToRigth_max = 3
elif (self.sub10 == "StabilizingRigth"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_StabilizingRigth_max):
self.sub10 = "GoneToRigth"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoneToRigth')
t_StabilizingRigth_max = 2
elif (self.sub10 == "GoneToRigth"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_GoneToRigth_max):
self.sub10 = "GoingToRigth"
t_activated = False
self.goneToRigth = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'GoingToRigth')
t_GoneToRigth_max = 0.1
# Actuation if
if (self.sub10 == "GoingToRigth"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = -0.75
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub10 == "StabilizingRigth"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdVelPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub10 == "GoneToRigth"):
self.goneToRigth = True
else:
if (self.sub10 == "GoingToRigth"):
t_GoingToRigth_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
self.sub10 = self.StatesSub10[ghostStateIndex]
elif (self.sub10 == "StabilizingRigth"):
t_StabilizingRigth_max = 2 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
self.sub10 = self.StatesSub10[ghostStateIndex]
elif (self.sub10 == "GoneToRigth"):
t_GoneToRigth_max = 0.1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
self.sub10 = self.StatesSub10[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to pose3d
pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if (not pose3d):
raise Exception('could not create proxy with pose3d')
self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
if (not self.pose3dPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'pose3d connected'
# Contact to cmdVel
cmdVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if (not cmdVel):
raise Exception('could not create proxy with cmdVel')
self.cmdVelPrx = CMDVelPrx.checkedCast(cmdVel)
if (not self.cmdVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'cmdVel connected'
# Contact to extra
extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if (not extra):
raise Exception('could not create proxy with extra')
self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
if (not self.extraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'extra connected'
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t2.join()
self.t3.join()
self.t4.join()
self.t5.join()
self.t6.join()
self.t8.join()
self.t9.join()
self.t10.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"PingPong",
"Numbers",
]
self.StatesSub3 = [
"Ping",
"Ping_ghost",
"Pong",
"Pong_ghost",
]
self.StatesSub4 = [
"1",
"1_ghost",
"2",
"2_ghost",
"3",
"3_ghost",
]
self.StatesSub5 = [
"wait2",
"wait2_ghost",
"wait1",
"wait1_ghost",
]
self.sub1 = "Numbers"
self.run1 = True
self.sub3 = "Ping_ghost"
self.run3 = True
self.sub4 = "1_ghost"
self.run4 = True
self.sub5 = "wait1_ghost"
self.run5 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t4 = threading.Thread(target=self.subautomata4)
self.t4.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 3, 113, 192, 0, 'PingPong')
guiSubautomata1.newGuiNode(2, 4, 512, 223, 1, 'Numbers')
guiSubautomata1.newGuiTransition((512, 223), (113, 192), (302, 378), 1,
2, 1)
guiSubautomata1.newGuiTransition((113, 192), (512, 223), (322, 127), 2,
1, 2)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3, 1, self.automataGui)
guiSubautomata3.newGuiNode(4, 0, 65, 154, 1, 'Ping')
guiSubautomata3.newGuiNode(5, 5, 313, 162, 0, 'Pong')
guiSubautomata3.newGuiTransition((65, 154), (313, 162), (192, 74), 3,
4, 5)
guiSubautomata3.newGuiTransition((313, 162), (65, 154), (187, 212), 4,
5, 4)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata4
guiSubautomata4 = GuiSubautomata(4, 2, self.automataGui)
guiSubautomata4.newGuiNode(6, 0, 161, 158, 1, '1')
guiSubautomata4.newGuiNode(7, 0, 493, 246, 0, '2')
guiSubautomata4.newGuiNode(8, 0, 207, 358, 0, '3')
guiSubautomata4.newGuiTransition((161, 158), (493, 246), (327, 202), 5,
6, 7)
guiSubautomata4.newGuiTransition((493, 246), (207, 358), (350, 302), 6,
7, 8)
guiSubautomata4.newGuiTransition((207, 358), (161, 158), (184, 258), 7,
8, 6)
guiSubautomataList.append(guiSubautomata4)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5, 5, self.automataGui)
guiSubautomata5.newGuiNode(9, 0, 257, 221, 0, 'wait2')
guiSubautomata5.newGuiNode(10, 0, 130, 105, 1, 'wait1')
guiSubautomata5.newGuiTransition((130, 105), (257, 221), (260, 99), 1,
10, 9)
guiSubautomata5.newGuiTransition((257, 221), (130, 105), (118, 214), 2,
9, 10)
guiSubautomataList.append(guiSubautomata5)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run4 = False
self.run5 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
self.numbersFinished = False
while (self.run1):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "PingPong"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 3.5):
self.sub1 = "Numbers"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Numbers')
elif (self.sub1 == "Numbers"):
if (self.numbersFinished):
self.sub1 = "PingPong"
self.numbersFinished = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('PingPong')
# Actuation if
if (self.sub1 == "PingPong"):
print "PingPong"
time.sleep(4)
elif (self.sub1 == "Numbers"):
print "Numbers"
time.sleep(5)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
self.ping = False
self.ping = False
while (self.run3):
totala = time.time() * 1000000
if (self.sub1 == "PingPong"):
if ((self.sub3 == "Ping_ghost")
or (self.sub3 == "Pong_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub3 == "Ping"):
if (self.ping):
self.sub3 = "Pong"
self.ping = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Pong')
elif (self.sub3 == "Pong"):
if (self.pong):
self.sub3 = "Ping"
self.pong = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Ping')
# Actuation if
if (self.sub3 == "Ping"):
print " PING"
time.sleep(0.5)
self.ping = True
elif (self.sub3 == "Pong"):
print " PONG"
time.sleep(0.5)
self.pong = True
else:
if (self.sub3 == "Ping"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif (self.sub3 == "Pong"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata4(self):
self.run4 = True
cycle = 100
t_activated = False
t_fin = 0
t_1_max = 0.001
t_2_max = 0.001
t_3_max = 0.001
while (self.run4):
totala = time.time() * 1000000
if (self.sub1 == "Numbers"):
if ((self.sub4 == "1_ghost") or (self.sub4 == "2_ghost")
or (self.sub4 == "3_ghost")):
ghostStateIndex = self.StatesSub4.index(self.sub4)
self.sub4 = self.StatesSub4[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub4 == "1"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_1_max):
self.sub4 = "2"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('2')
t_1_max = 0.001
elif (self.sub4 == "2"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_2_max):
self.sub4 = "3"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('3')
t_2_max = 0.001
elif (self.sub4 == "3"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_3_max):
self.sub4 = "1"
t_activated = False
self.numbersFinished = True
if self.displayGui:
self.automataGui.notifySetNodeAsActive('1')
t_3_max = 0.001
# Actuation if
if (self.sub4 == "1"):
print " 1"
time.sleep(1)
elif (self.sub4 == "2"):
print " 2"
time.sleep(1)
elif (self.sub4 == "3"):
print " 3"
time.sleep(1)
else:
if (self.sub4 == "1"):
t_1_max = 0.001 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif (self.sub4 == "2"):
t_2_max = 0.001 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif (self.sub4 == "3"):
t_3_max = 0.001 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata5(self):
self.run5 = True
cycle = 100
t_activated = False
t_fin = 0
t_wait1_max = 1
t_wait2_max = 1
while (self.run5):
totala = time.time() * 1000000
if (self.sub3 == "Pong"):
if ((self.sub5 == "wait2_ghost")
or (self.sub5 == "wait1_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub5 == "wait2"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_wait2_max):
self.sub5 = "wait1"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('wait1')
t_wait2_max = 1
elif (self.sub5 == "wait1"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_wait1_max):
self.sub5 = "wait2"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('wait2')
t_wait1_max = 1
# Actuation if
if (self.sub5 == "wait2"):
print ""
elif (self.sub5 == "wait1"):
print ""
else:
if (self.sub5 == "wait2"):
t_wait2_max = 1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "wait1"):
t_wait1_max = 1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t4.join()
self.t5.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"goforward",
]
self.sub1 = "goforward"
self.run1 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 118, 121, 1, 'goforward')
guiSubautomataList.append(guiSubautomata1)
return guiSubautomataList
def shutDown(self):
self.run1 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
# Actuation if
if(self.sub1 == "goforward"):
self.my_motors.sendV(0.2)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = EasyIce.initialize(sys.argv)
self.ic,self.node = comm.init(self.ic)
# Contact to my_motors
self.my_motors = comm.getMotorsClient(self.ic, 'automata.my_motors')
if(not self.my_motors):
raise Exception('could not create client with my_motors')
print('my_motors connected')
def destroyIc(self):
self.my_motors.stop()
comm.destroy(self.ic, self.node)
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print('runtime gui enabled')
else:
self.displayGui = False
print('runtime gui disabled')
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.StatesSub1 = [
"TakeOff",
"GoFront",
"Turn",
"Land",
"Finish",
]
self.StatesSub2 = [
"TakingOff",
"TakingOff_ghost",
"Stabilizing",
"Stabilizing_ghost",
"ENDSUBAUTOMATA",
"ENDSUBAUTOMATA_ghost",
]
self.StatesSub3 = [
"GoingFront",
"GoingFront_ghost",
"Stabilizing",
"Stabilizing_ghost",
"ENDSUBAUTOMATA",
"ENDSUBAUTOMATA_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub2 = "TakingOff_ghost"
self.run2 = True
self.sub3 = "GoingFront_ghost"
self.run3 = True
def shutDown(self):
self.run1 = False
self.run2 = False
self.run3 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.draw()
self.automataGui.show()
app.exec_()
def subautomata1(self):
run = True
cycle = 100
t_activated = False
self.endTakeOff = False
self.endGoFront = False
hasLanded = False
while(run):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "TakeOff"):
if(self.endTakeOff):
self.sub1 = "GoFront"
elif(self.sub1 == "GoFront"):
if(self.endGoFront):
self.sub1 = "Turn"
elif(self.sub1 == "Turn"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 0.5):
self.sub1 = "Land"
t_activated = False
elif(self.sub1 == "Land"):
if(hasLanded):
self.sub1 = "Finish"
# Actuation if
if(self.sub1 == "GoFront"):
print "Going Front"
elif(self.sub1 == "Turn"):
print "Do nothing yet"
elif(self.sub1 == "Land"):
self.lock.acquire()
self.extraPrx.land()
self.lock.release()
hasLanded = True
elif(self.sub1 == "Finish"):
print "Finished!"
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata2(self):
run = True
cycle = 100
t_activated = False
t_Stabilizing_max = 2.5
hasTakenOff = False
while(run):
totala = time.time() * 1000000
if(self.sub1 == "TakeOff"):
if ((self.sub2 == "TakingOff_ghost") or (self.sub2 == "Stabilizing_ghost") or (self.sub2 == "ENDSUBAUTOMATA_ghost")):
ghostStateIndex = self.StatesSub2.index(self.sub2)
self.sub2 = self.StatesSub2[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub2 == "TakingOff"):
if(hasTakenOff):
self.sub2 = "Stabilizing"
elif(self.sub2 == "Stabilizing"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_Stabilizing_max):
self.sub2 = "ENDSUBAUTOMATA"
t_activated = False
t_Stabilizing_max = 2.5
# Actuation if
if(self.sub2 == "TakingOff"):
self.lock.acquire()
self.extraPrx.takeoff()
self.lock.release()
hasTakenOff = True
print "TakingOff"
elif(self.sub2 == "Stabilizing"):
print "Stabilizing after taking off"
elif(self.sub2 == "ENDSUBAUTOMATA"):
if(not self.endTakeOff):
print "TakeOff ended"
self.endTakeOff = True
else:
if(self.sub2 == "Stabilizing"):
t_Stabilizing_max = 2.5 - (t_fin - t_ini)
ghostStateIndex = self.StateSub2.index(self.sub2) + 1
sub2 = self.StatesSub2[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata3(self):
run = True
cycle = 100
t_activated = False
t_GoingFront_max = 4
t_Stabilizing_max = 3
while(run):
totala = time.time() * 1000000
if(self.sub1 == "GoFront"):
if ((self.sub3 == "GoingFront_ghost") or (self.sub3 == "Stabilizing_ghost") or (self.sub3 == "ENDSUBAUTOMATA_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub3 == "GoingFront"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_GoingFront_max):
self.sub3 = "Stabilizing"
t_activated = False
t_GoingFront_max = 4
elif(self.sub3 == "Stabilizing"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_Stabilizing_max):
self.sub3 = "ENDSUBAUTOMATA"
t_activated = False
t_Stabilizing_max = 3
# Actuation if
if(self.sub3 == "GoingFront"):
cmd = jderobot.CMDVelData()
cmd.linearX = 1
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub3 == "ENDSUBAUTOMATA"):
self.endGoFront = True
else:
if(self.sub3 == "GoingFront"):
t_GoingFront_max = 4 - (t_fin - t_ini)
ghostStateIndex = self.StateSub3.index(self.sub3) + 1
sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "Stabilizing"):
t_Stabilizing_max = 3 - (t_fin - t_ini)
ghostStateIndex = self.StateSub3.index(self.sub3) + 1
sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to cmd
cmd = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if(not cmd):
raise Exception('could not create proxy with cmd')
self.cmdPrx = CMDVelPrx.checkedCast(cmd)
if(not self.cmdPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'cmd connected'
# Contact to extra
extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if(not extra):
raise Exception('could not create proxy with extra')
self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
if(not self.extraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'extra connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
self.t1 = threading.Thread(target=self.subautomata1)
#self.t1.start()
self.t2 = threading.Thread(target=self.subautomata2)
#self.t2.start()
self.t3 = threading.Thread(target=self.subautomata3)
#self.t3.start()
def join(self):
self.guiThread.join()
self.t1.join()
self.t2.join()
self.t3.join()
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"FollowGreenLookBlue",
"FollowBlueLookGreen",
"FollowGreenLookRed",
"FollowRed",
]
self.StatesSub3 = [
"WaitGreen",
"WaitGreen_ghost",
"FollowGreen",
"FollowGreen_ghost",
]
self.StatesSub4 = [
"WaitBlue",
"WaitBlue_ghost",
"FollowBlue",
"FollowBlue_ghost",
]
self.StatesSub5 = [
"WaitGreen2",
"WaitGreen2_ghost",
"FollowGreen2",
"FollowGreen2_ghost",
]
self.StatesSub6 = [
"WaitRed",
"WaitRed_ghost",
"FollowingRed",
"FollowingRed_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub3 = "WaitGreen_ghost"
self.run3 = True
self.sub4 = "WaitBlue_ghost"
self.run4 = True
self.sub5 = "WaitGreen2_ghost"
self.run5 = True
self.sub6 = "WaitRed_ghost"
self.run6 = True
def getImage(self):
img = self.CameraPrx.getImageData("RGB8")
height = img.description.height
width = img.description.width
if self.targetX == None and self.targetY == None:
self.targetX = width / 2
self.targetY = height / 2
print "targetX:", self.targetX, "targetY:,", self.targetY
imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
imgPixels.shape = height, width, 3
return imgPixels
def getContours(self, img, minValues, maxValues):
hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)
contours, hierarchy = cv2.findContours(thresholdImg, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
return contours, hierarchy
def getVelocities(self, contours):
contour = max(contours, key=cv2.contourArea)
(x, y), radius = cv2.minEnclosingCircle(contour)
xError = self.targetX - x
yError = self.targetY - y
xVel = self.xPID.process(xError)
yVel = self.yPID.process(yError)
return xVel, yVel
def setVelocity(self, vx, vy, vz, yaw):
cmd = jderobot.CMDVelData()
cmd.linearX = vy
cmd.linearY = vx
cmd.linearZ = vz
cmd.angularZ = yaw
cmd.angularX = cmd.angularY = 1.0
self.CMDVelPrx.setCMDVelData(cmd)
class PID:
def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
self.Ep = Epsilon
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.Imax = Imax
self.Imin = Imin
self.lastError = 0
self.cumulativeError = 0
def updateCumulativeError(self, error):
self.cumulativeError += error
if self.cumulativeError > self.Imax:
self.cumulativeError = self.Imax
elif self.cumulativeError < self.Imin:
self.cumulativeError = self.Imin
def process(self, error, derivative=None, integral=None):
if -self.Ep < error < self.Ep:
return 0
P = self.Kp * error
self.updateCumulativeError(error)
if integral != None:
I = self.Ki * integral
else:
I = self.Ki * self.cumulativeError
if derivative != None:
D = self.Kd * derivative
else:
D = self.Kd * (error - self.lastError)
self.lastError = error
speed = P + I + D
#if speed > 0.4:
# speed = 0.4
#if speed < -0.4:
# speed = -0.4
return speed
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t4 = threading.Thread(target=self.subautomata4)
self.t4.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
self.t6 = threading.Thread(target=self.subautomata6)
self.t6.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 117, 139, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 3, 303, 150, 0, 'FollowGreenLookBlue')
guiSubautomata1.newGuiNode(5, 4, 421, 244, 0, 'FollowBlueLookGreen')
guiSubautomata1.newGuiNode(6, 5, 325, 312, 0, 'FollowGreenLookRed')
guiSubautomata1.newGuiNode(7, 6, 142, 326, 0, 'FollowRed')
guiSubautomata1.newGuiTransition((303, 150), (421, 244), (421, 157), 4,
2, 5)
guiSubautomata1.newGuiTransition((421, 244), (325, 312), (436, 325), 5,
5, 6)
guiSubautomata1.newGuiTransition((325, 312), (142, 326), (232, 331), 6,
6, 7)
guiSubautomata1.newGuiTransition((117, 139), (303, 150), (194, 174),
15, 1, 2)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3, 2, self.automataGui)
guiSubautomata3.newGuiNode(3, 0, 67, 147, 1, 'WaitGreen')
guiSubautomata3.newGuiNode(4, 0, 269, 152, 0, 'FollowGreen')
guiSubautomata3.newGuiTransition((67, 147), (269, 152), (173, 40), 2,
3, 4)
guiSubautomata3.newGuiTransition((269, 152), (67, 147), (170, 221), 3,
4, 3)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata4
guiSubautomata4 = GuiSubautomata(4, 5, self.automataGui)
guiSubautomata4.newGuiNode(8, 0, 111, 129, 1, 'WaitBlue')
guiSubautomata4.newGuiNode(9, 0, 296, 141, 0, 'FollowBlue')
guiSubautomata4.newGuiTransition((111, 129), (296, 141), (200, 60), 7,
8, 9)
guiSubautomata4.newGuiTransition((296, 141), (111, 129), (191, 198), 8,
9, 8)
guiSubautomataList.append(guiSubautomata4)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5, 6, self.automataGui)
guiSubautomata5.newGuiNode(10, 0, 79, 120, 1, 'WaitGreen2')
guiSubautomata5.newGuiNode(11, 0, 336, 152, 0, 'FollowGreen2')
guiSubautomata5.newGuiTransition((336, 152), (79, 120), (196, 220), 9,
11, 10)
guiSubautomata5.newGuiTransition((79, 120), (336, 152), (217, 56), 10,
10, 11)
guiSubautomataList.append(guiSubautomata5)
# Creating subAutomata6
guiSubautomata6 = GuiSubautomata(6, 7, self.automataGui)
guiSubautomata6.newGuiNode(12, 0, 104, 151, 1, 'WaitRed')
guiSubautomata6.newGuiNode(13, 0, 341, 163, 0, 'FollowingRed')
guiSubautomata6.newGuiTransition((104, 151), (341, 163), (221, 56), 11,
12, 13)
guiSubautomata6.newGuiTransition((341, 163), (104, 151), (225, 219),
12, 13, 12)
guiSubautomataList.append(guiSubautomata6)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run4 = False
self.run5 = False
self.run6 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
#ControlValues
minError = 10
self.targetX = None
self.targetY = None
self.hIters = 0
#maxHIters = 35 #For gazebo
maxHIters = 20 #For real
refracIters = 0
refracTime = 35
#Contours
self.greenConts = []
self.blueConts = []
self.redConts = []
#Filter values for simulator. Order: [H, S, V]
minGValues = numpy.array([30, 129, 33])
maxGValues = numpy.array([70, 255, 190])
minBValues = numpy.array([0, 140, 37])
maxBValues = numpy.array([16, 255, 200])
minRValues = numpy.array([100, 209, 82])
maxRValues = numpy.array([153, 255, 200])
#Filter values for real. Order: [H, S, V]
#minGValues = numpy.array([31,0,120])
#maxGValues = numpy.array([70, 46 , 255])
#minBValues = numpy.array([9,126,175])
#maxBValues = numpy.array([58,209,255])
#minRValues = numpy.array([93,195,154])
#maxRValues = numpy.array([155,255,255])
#PID control
self.xPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)
self.yPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)
while (self.run1):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "TakeOff"):
if (self.hIters >= maxHIters):
self.sub1 = "FollowGreenLookBlue"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowGreenLookBlue')
elif (self.sub1 == "FollowGreenLookBlue"):
if (self.blueConts):
self.sub1 = "FollowBlueLookGreen"
print "Reseting Green Conts"
self.greenConts = []
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowBlueLookGreen')
elif (self.sub1 == "FollowBlueLookGreen"):
if (self.greenConts):
self.sub1 = "FollowGreenLookRed"
self.blueConts = []
refracIters = 0
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowGreenLookRed')
elif (self.sub1 == "FollowGreenLookRed"):
if (self.redConts):
self.sub1 = "FollowRed"
self.greenConts = []
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowRed')
# Actuation if
if (self.sub1 == "TakeOff"):
if self.hIters == 0:
self.ExtraPrx.takeoff()
print "taking off"
else:
#self.setVelocity(0,0,0.5,0)
self.setVelocity(0, 0, 1, 0)
self.hIters += 1
print "ITERS: ", self.hIters
elif (self.sub1 == "FollowGreenLookBlue"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.greenConts, hierarchy = self.getContours(
softenedImg, minGValues, maxGValues)
self.blueConts, hierarchy = self.getContours(
softenedImg, minBValues, maxBValues)
elif (self.sub1 == "FollowBlueLookGreen"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.blueConts, hierarchy = self.getContours(
softenedImg, minBValues, maxBValues)
if refracIters >= refracTime:
self.greenConts, hierarchy = self.getContours(
softenedImg, minGValues, maxGValues)
print "inside refrac time"
else:
print "refrac Iters:", refracIters
refracIters += 1
elif (self.sub1 == "FollowGreenLookRed"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.greenConts, hierarchy = self.getContours(
softenedImg, minGValues, maxGValues)
self.redConts, hierarchy = self.getContours(
softenedImg, minRValues, maxRValues)
print "RED:", self.redConts
elif (self.sub1 == "FollowRed"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.redConts, hierarchy = self.getContours(
softenedImg, minRValues, maxRValues)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run3):
totala = time.time() * 1000000
if (self.sub1 == "FollowGreenLookBlue"):
if ((self.sub3 == "WaitGreen_ghost")
or (self.sub3 == "FollowGreen_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub3 == "WaitGreen"):
if (self.greenConts):
self.sub3 = "FollowGreen"
print "Green Finded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowGreen')
elif (self.sub3 == "FollowGreen"):
if (not self.greenConts):
self.sub3 = "WaitGreen"
print "Green Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('WaitGreen')
# Actuation if
if (self.sub3 == "WaitGreen"):
self.setVelocity(0, 0, 0, 0)
elif (self.sub3 == "FollowGreen"):
xVel, yVel = self.getVelocities(self.greenConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if (self.sub3 == "WaitGreen"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif (self.sub3 == "FollowGreen"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata4(self):
self.run4 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run4):
totala = time.time() * 1000000
if (self.sub1 == "FollowBlueLookGreen"):
if ((self.sub4 == "WaitBlue_ghost")
or (self.sub4 == "FollowBlue_ghost")):
ghostStateIndex = self.StatesSub4.index(self.sub4)
self.sub4 = self.StatesSub4[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub4 == "WaitBlue"):
if (self.blueConts):
self.sub4 = "FollowBlue"
print "Blue Finded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowBlue')
elif (self.sub4 == "FollowBlue"):
if (not self.blueConts):
self.sub4 = "WaitBlue"
print "Blue Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('WaitBlue')
# Actuation if
if (self.sub4 == "WaitBlue"):
self.setVelocity(0, 0, 0, 0)
elif (self.sub4 == "FollowBlue"):
xVel, yVel = self.getVelocities(self.blueConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if (self.sub4 == "WaitBlue"):
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif (self.sub4 == "FollowBlue"):
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata5(self):
self.run5 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run5):
totala = time.time() * 1000000
if (self.sub1 == "FollowGreenLookRed"):
if ((self.sub5 == "WaitGreen2_ghost")
or (self.sub5 == "FollowGreen2_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub5 == "WaitGreen2"):
if (self.greenConts):
self.sub5 = "FollowGreen2"
print "Green2 finded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowGreen2')
elif (self.sub5 == "FollowGreen2"):
if (not self.greenConts):
self.sub5 = "WaitGreen2"
print "Green2 Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'WaitGreen2')
# Actuation if
if (self.sub5 == "WaitGreen2"):
self.setVelocity(0, 0, 0, 0)
elif (self.sub5 == "FollowGreen2"):
xVel, yVel = self.getVelocities(self.greenConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if (self.sub5 == "WaitGreen2"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "FollowGreen2"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata6(self):
self.run6 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run6):
totala = time.time() * 1000000
if (self.sub1 == "FollowRed"):
if ((self.sub6 == "WaitRed_ghost")
or (self.sub6 == "FollowingRed_ghost")):
ghostStateIndex = self.StatesSub6.index(self.sub6)
self.sub6 = self.StatesSub6[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub6 == "WaitRed"):
if (self.redConts):
self.sub6 = "FollowingRed"
print "Red FInded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowingRed')
elif (self.sub6 == "FollowingRed"):
if (not self.redConts):
self.sub6 = "WaitRed"
print "Red Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('WaitRed')
# Actuation if
if (self.sub6 == "WaitRed"):
self.setVelocity(0, 0, 0, 0)
elif (self.sub6 == "FollowingRed"):
xVel, yVel = self.getVelocities(self.redConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if (self.sub6 == "WaitRed"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif (self.sub6 == "FollowingRed"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Extra
Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if (not Extra):
raise Exception('could not create proxy with Extra')
self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
if (not self.ExtraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'Extra connected'
# Contact to CMDVel
CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if (not CMDVel):
raise Exception('could not create proxy with CMDVel')
self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
if (not self.CMDVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'CMDVel connected'
# Contact to Camera
Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if (not Camera):
raise Exception('could not create proxy with Camera')
self.CameraPrx = CameraPrx.checkedCast(Camera)
if (not self.CameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'Camera connected'
# Contact to Pose3D
Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if (not Pose3D):
raise Exception('could not create proxy with Pose3D')
self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
if (not self.Pose3DPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'Pose3D connected'
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t4.join()
self.t5.join()
self.t6.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"FollowRoad",
"MonitorArea",
"TurnAround",
"Landing",
"END",
"HeighControl",
]
self.StatesSub3 = [
"FindRoad",
"FindRoad_ghost",
"FollowingRoad",
"FollowingRoad_ghost",
]
self.StatesSub5 = [
"ToFirstPos",
"ToFirstPos_ghost",
"ToSecondPos",
"ToSecondPos_ghost",
"ToThirdPos",
"ToThirdPos_ghost",
"Return",
"Return_ghost",
]
self.StatesSub6 = [
"Descending",
"Descending_ghost",
"Land",
"Land_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub3 = "FindRoad_ghost"
self.run3 = True
self.sub5 = "ToFirstPos_ghost"
self.run5 = True
self.sub6 = "Descending_ghost"
self.run6 = True
def getImage(self):
img = self.CameraPrx.getImageData("RGB8")
height = img.description.height
width=img.description.width
imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
imgPixels.shape = height, width, 3
return imgPixels
def setVelocity(self, vx, vy, vz, yaw):
cmd = jderobot.CMDVelData()
cmd.linearX = vy
cmd.linearY = vx
cmd.linearZ = vz
cmd.angularZ = yaw
cmd.angularX = cmd.angularY = 1.0
self.CMDVelPrx.setCMDVelData(cmd)
#The factor indicate the margin of the error multipliyin the error for this factor
def droneInPosition(self, pos,factor=1 ):
return (abs(pos[0] - self.xPos) < self.minDist*factor) and (abs(pos[1] - self.yPos) < self.minDist*factor)
class PID:
def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
self.Ep = Epsilon
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.Imax = Imax
self.Imin = Imin
self.lastError = 0
self.cumulativeError = 0
def updateCumulativeError(self, error):
self.cumulativeError += error
if self.cumulativeError > self.Imax:
self.cumulativeError = self.Imax
elif self.cumulativeError < self.Imin:
self.cumulativeError = self.Imin
def process(self, error, derivative=None, integral=None):
if -self.Ep < error < self.Ep:
return 0
P = self.Kp * error
self.updateCumulativeError(error)
if integral != None:
I = self.Ki * integral
else:
I = self.Ki * self.cumulativeError
if derivative != None:
D = self.Kd * derivative
else:
D = self.Kd * (error - self.lastError)
self.lastError = error
speed = P + I + D
if speed > 3:
speed = 3
elif speed < -3:
speed = -3
return speed
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
self.t6 = threading.Thread(target=self.subautomata6)
self.t6.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 62, 66, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 3, 189, 116, 0, 'FollowRoad')
guiSubautomata1.newGuiNode(6, 5, 309, 268, 0, 'MonitorArea')
guiSubautomata1.newGuiNode(11, 0, 263, 428, 0, 'TurnAround')
guiSubautomata1.newGuiNode(12, 6, 53, 239, 0, 'Landing')
guiSubautomata1.newGuiNode(14, 0, 41, 427, 0, 'END')
guiSubautomata1.newGuiNode(17, 0, 481, 139, 0, 'HeighControl')
guiSubautomata1.newGuiTransition((189, 116), (309, 268), (274, 172), 7, 2, 6)
guiSubautomata1.newGuiTransition((309, 268), (263, 428), (349, 362), 13, 6, 11)
guiSubautomata1.newGuiTransition((263, 428), (189, 116), (164, 318), 14, 11, 2)
guiSubautomata1.newGuiTransition((189, 116), (53, 239), (82, 154), 19, 2, 12)
guiSubautomata1.newGuiTransition((53, 239), (41, 427), (43, 326), 22, 12, 14)
guiSubautomata1.newGuiTransition((481, 139), (189, 116), (328, 116), 26, 17, 2)
guiSubautomata1.newGuiTransition((62, 66), (481, 139), (430, 27), 27, 1, 17)
guiSubautomata1.newGuiTransition((189, 116), (481, 139), (315, 70), 28, 2, 17)
guiSubautomata1.newGuiTransition((309, 268), (481, 139), (378, 195), 29, 6, 17)
guiSubautomata1.newGuiTransition((481, 139), (309, 268), (412, 279), 30, 17, 6)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3,2, self.automataGui)
guiSubautomata3.newGuiNode(3, 0, 156, 228, 1, 'FindRoad')
guiSubautomata3.newGuiNode(4, 0, 427, 255, 0, 'FollowingRoad')
guiSubautomata3.newGuiTransition((156, 228), (427, 255), (297, 157), 2, 3, 4)
guiSubautomata3.newGuiTransition((427, 255), (156, 228), (265, 320), 3, 4, 3)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5,6, self.automataGui)
guiSubautomata5.newGuiNode(7, 0, 86, 275, 1, 'ToFirstPos')
guiSubautomata5.newGuiNode(8, 0, 247, 92, 0, 'ToSecondPos')
guiSubautomata5.newGuiNode(9, 0, 491, 303, 0, 'ToThirdPos')
guiSubautomata5.newGuiNode(10, 0, 281, 455, 0, 'Return')
guiSubautomata5.newGuiTransition((86, 275), (247, 92), (166, 184), 9, 7, 8)
guiSubautomata5.newGuiTransition((247, 92), (491, 303), (369, 198), 10, 8, 9)
guiSubautomata5.newGuiTransition((491, 303), (281, 455), (386, 379), 11, 9, 10)
guiSubautomata5.newGuiTransition((281, 455), (86, 275), (184, 365), 12, 10, 7)
guiSubautomataList.append(guiSubautomata5)
# Creating subAutomata6
guiSubautomata6 = GuiSubautomata(6,12, self.automataGui)
guiSubautomata6.newGuiNode(15, 0, 126, 185, 1, 'Descending')
guiSubautomata6.newGuiNode(16, 0, 350, 190, 0, 'Land')
guiSubautomata6.newGuiTransition((126, 185), (350, 190), (232, 220), 24, 15, 16)
guiSubautomataList.append(guiSubautomata6)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run5 = False
self.run6 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
self.targetX = 125
self.targetY = 125
self.targetZ = 2.5
self.initPos = (-1, -8.5)
landingPose = (self.Pose3DPrx.getPose3DData().x, self.Pose3DPrx.getPose3DData().y)
#Minimun errors
self.minError = 10
self.minActit = 0.5
self.minAltit = 0.01
self.minDist = 0.01
heighMargin = 1
#Control Variables
self.heigh = 0
self.contours = []
self.monitorComplet = False
self.xPos = landingPose[0]
self.yPos = landingPose[1]
initAngle = None
self.hasLanded = False
currentState = "FollowRoad"
takenOff = False
#Filter Values
self.hmin = 90
self.hmax = 97
self.vmin = 0
self.vmax = 50
self.smin = 45
self.smax = 80
#Control PID
self.xPid = self.PID(Epsilon=self.minError, Kp=0.01, Ki=0.04, Kd=0.003, Imax=5, Imin=-5)
self.zPid = self.PID(Epsilon=self.minAltit, Kp=1, Ki=0.02, Kd=0, Imax=5, Imin=-5)
self.aPid = self.PID(Epsilon=self.minActit, Kp=0.01, Ki=0.04, Kd=0.003, Imax=5, Imin=-5)
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "TakeOff"):
if(takenOff):
self.sub1 = "HeighControl"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('HeighControl')
elif(self.sub1 == "FollowRoad"):
if((self.droneInPosition(self.initPos, 200)) and (not self.monitorComplet)):
self.sub1 = "MonitorArea"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('MonitorArea')
elif((self.droneInPosition(landingPose, 75)) and (self.monitorComplet)):
self.sub1 = "Landing"
self.setVelocity(0,0,0,0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Landing')
elif(abs(self.targetZ - self.heigh) > heighMargin):
self.sub1 = "HeighControl"
currentState = "FollowRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('HeighControl')
elif(self.sub1 == "MonitorArea"):
if(self.monitorComplet):
self.sub1 = "TurnAround"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('TurnAround')
elif(abs(self.targetZ - self.heigh) > heighMargin):
self.sub1 = "HeighControl"
currentState = "MonitorArea"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('HeighControl')
elif(self.sub1 == "TurnAround"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 2.5):
self.sub1 = "FollowRoad"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowRoad')
elif(self.sub1 == "Landing"):
if(self.hasLanded):
self.sub1 = "END"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('END')
elif(self.sub1 == "HeighControl"):
if((abs(self.targetZ - self.heigh) < self.minAltit) and (currentState == "FollowRoad")):
self.sub1 = "FollowRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowRoad')
elif((abs(self.targetZ - self.heigh) < self.minAltit) and (currentState == "MonitorArea")):
self.sub1 = "MonitorArea"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('MonitorArea')
# Actuation if
if(self.sub1 == "TakeOff"):
#Taking Off
self.ExtraPrx.takeoff()
takenOff = True
elif(self.sub1 == "FollowRoad"):
lastState = "FollowRoad"
#Get and prepare image
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
hsvImg = cv2.cvtColor(softenedImg, cv2.COLOR_BGR2HSV)
#Get threshold image
minValues = numpy.array([self.hmin, self.vmin, self.smin])
maxValues = numpy.array([self.hmax, self.vmax, self.smax])
thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)
#Get contours
self.contours, hierarchy = cv2.findContours(thresholdImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
#Update Heigh
self.heigh = self.Pose3DPrx.getPose3DData().z
elif(self.sub1 == "TurnAround"):
self.setVelocity(0, 0, 0, 1)
pose= self.Pose3DPrx.getPose3DData()
print "q0", pose.q0, "q1",pose.q1, "q2",pose.q2, "q3",pose.q3
elif(self.sub1 == "END"):
print "SHUT DOWN"
self.shutDown()
elif(self.sub1 == "HeighControl"):
#Controling heigh
self.heigh = self.Pose3DPrx.getPose3DData().z
zVel = self.zPid.process(self.targetZ - self.heigh)
self.setVelocity(0, 0, zVel, 0)
print "Heigh:", self.heigh
print "Vel:", zVel
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run3):
totala = time.time() * 1000000
if(self.sub1 == "FollowRoad"):
if ((self.sub3 == "FindRoad_ghost") or (self.sub3 == "FollowingRoad_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub3 == "FindRoad"):
if(self.contours):
self.sub3 = "FollowingRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowingRoad')
elif(self.sub3 == "FollowingRoad"):
if(not self.contours):
self.sub3 = "FindRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FindRoad')
# Actuation if
if(self.sub3 == "FindRoad"):
self.setVelocity(0, 0, 0.75, 0)
print "Road lost"
elif(self.sub3 == "FollowingRoad"):
contour = max(self.contours, key=cv2.contourArea)
try:
(x, y), radius = cv2.minEnclosingCircle(contour)
center = (int(x), int(y))
ellipse = cv2.fitEllipse(contour)
inclination = ellipse[2]
if inclination < 90:
yAngle = -inclination
else:
yAngle = 180 - inclination
avel = self.aPid.process(yAngle)
xError = self.targetX - center[0]
xvel = self.xPid.process(xError)
speed = max((self.targetX - pow(1.09, abs(xError)))/125.,0)
self.setVelocity(xvel, speed, 0, avel)
except:
print "EXCEPTION FOUND"
self.contours = []
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
print "Xpos:", self.xPos, "Ypos:", self.yPos
else:
if(self.sub3 == "FindRoad"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "FollowingRoad"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata5(self):
self.run5 = True
cycle = 100
t_activated = False
t_fin = 0
firstPos = (10, -20)
secondPos = (5, -30)
thirdPos = (-10, -20)
self.minDist = 0.02
#Control PID
xDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)
yDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)
while(self.run5):
totala = time.time() * 1000000
if(self.sub1 == "MonitorArea"):
if ((self.sub5 == "ToFirstPos_ghost") or (self.sub5 == "ToSecondPos_ghost") or (self.sub5 == "ToThirdPos_ghost") or (self.sub5 == "Return_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub5 == "ToFirstPos"):
if(self.droneInPosition(firstPos)):
self.sub5 = "ToSecondPos"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('ToSecondPos')
elif(self.sub5 == "ToSecondPos"):
if(self.droneInPosition(secondPos)):
self.sub5 = "ToThirdPos"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('ToThirdPos')
elif(self.sub5 == "ToThirdPos"):
if(self.droneInPosition(thirdPos)):
self.sub5 = "Return"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Return')
elif(self.sub5 == "Return"):
if(self.droneInPosition(self.initPos)):
self.sub5 = "ToFirstPos"
self.monitorComplet = True
if self.displayGui:
self.automataGui.notifySetNodeAsActive('ToFirstPos')
# Actuation if
if(self.sub5 == "ToFirstPos"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = firstPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = firstPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "Xerror:", xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
elif(self.sub5 == "ToSecondPos"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = secondPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = secondPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "xError:",xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
elif(self.sub5 == "ToThirdPos"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = thirdPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = thirdPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "Xerror:", xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", yVel
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
elif(self.sub5 == "Return"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = self.initPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = self.initPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "Xerror:", xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
else:
if(self.sub5 == "ToFirstPos"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "ToSecondPos"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "ToThirdPos"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "Return"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata6(self):
self.run6 = True
cycle = 100
t_activated = False
t_fin = 0
targetZDescend = 0.5
while(self.run6):
totala = time.time() * 1000000
if(self.sub1 == "Landing"):
if ((self.sub6 == "Descending_ghost") or (self.sub6 == "Land_ghost")):
ghostStateIndex = self.StatesSub6.index(self.sub6)
self.sub6 = self.StatesSub6[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub6 == "Descending"):
if(abs(targetZDescend - self.heigh) < self.minAltit):
self.sub6 = "Land"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Land')
# Actuation if
if(self.sub6 == "Descending"):
#Controling heigh
self.heigh = self.Pose3DPrx.getPose3DData().z
zVel = self.zPid.process(targetZDescend - self.heigh)
self.setVelocity(0, 0, zVel, 0)
print "Heigh:", self.heigh
elif(self.sub6 == "Land"):
self.setVelocity(0,0,0,0)
if not self.hasLanded:
self.ExtraPrx.land()
self.hasLanded =True
else:
if(self.sub6 == "Descending"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif(self.sub6 == "Land"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Extra
Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if(not Extra):
raise Exception('could not create proxy with Extra')
self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
if(not self.ExtraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'Extra connected'
# Contact to CMDVel
CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if(not CMDVel):
raise Exception('could not create proxy with CMDVel')
self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
if(not self.CMDVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'CMDVel connected'
# Contact to Pose3D
Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if(not Pose3D):
raise Exception('could not create proxy with Pose3D')
self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
if(not self.Pose3DPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'Pose3D connected'
# Contact to Camera
Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if(not Camera):
raise Exception('could not create proxy with Camera')
self.CameraPrx = CameraPrx.checkedCast(Camera)
if(not self.CameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'Camera connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t5.join()
self.t6.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"PingPong",
"Numbers",
]
self.StatesSub3 = [
"Ping",
"Ping_ghost",
"Pong",
"Pong_ghost",
]
self.StatesSub4 = [
"1",
"1_ghost",
"2",
"2_ghost",
"3",
"3_ghost",
]
self.StatesSub5 = [
"wait2",
"wait2_ghost",
"wait1",
"wait1_ghost",
]
self.sub1 = "Numbers"
self.run1 = True
self.sub3 = "Ping_ghost"
self.run3 = True
self.sub4 = "1_ghost"
self.run4 = True
self.sub5 = "wait1_ghost"
self.run5 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t4 = threading.Thread(target=self.subautomata4)
self.t4.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 3, 113, 192, 0, 'PingPong')
guiSubautomata1.newGuiNode(2, 4, 512, 223, 1, 'Numbers')
guiSubautomata1.newGuiTransition((512, 223), (113, 192), (302, 378), 1, 2, 1)
guiSubautomata1.newGuiTransition((113, 192), (512, 223), (322, 127), 2, 1, 2)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3,1, self.automataGui)
guiSubautomata3.newGuiNode(4, 0, 65, 154, 1, 'Ping')
guiSubautomata3.newGuiNode(5, 5, 313, 162, 0, 'Pong')
guiSubautomata3.newGuiTransition((65, 154), (313, 162), (192, 74), 3, 4, 5)
guiSubautomata3.newGuiTransition((313, 162), (65, 154), (187, 212), 4, 5, 4)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata4
guiSubautomata4 = GuiSubautomata(4,2, self.automataGui)
guiSubautomata4.newGuiNode(6, 0, 161, 158, 1, '1')
guiSubautomata4.newGuiNode(7, 0, 493, 246, 0, '2')
guiSubautomata4.newGuiNode(8, 0, 207, 358, 0, '3')
guiSubautomata4.newGuiTransition((161, 158), (493, 246), (327, 202), 5, 6, 7)
guiSubautomata4.newGuiTransition((493, 246), (207, 358), (350, 302), 6, 7, 8)
guiSubautomata4.newGuiTransition((207, 358), (161, 158), (184, 258), 7, 8, 6)
guiSubautomataList.append(guiSubautomata4)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5,5, self.automataGui)
guiSubautomata5.newGuiNode(9, 0, 257, 221, 0, 'wait2')
guiSubautomata5.newGuiNode(10, 0, 130, 105, 1, 'wait1')
guiSubautomata5.newGuiTransition((130, 105), (257, 221), (260, 99), 1, 10, 9)
guiSubautomata5.newGuiTransition((257, 221), (130, 105), (118, 214), 2, 9, 10)
guiSubautomataList.append(guiSubautomata5)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run4 = False
self.run5 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
self.numbersFinished = False
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "PingPong"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3.5):
self.sub1 = "Numbers"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Numbers')
elif(self.sub1 == "Numbers"):
if(self.numbersFinished):
self.sub1 = "PingPong"
self.numbersFinished = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('PingPong')
# Actuation if
if(self.sub1 == "PingPong"):
print "PingPong"
time.sleep(4)
elif(self.sub1 == "Numbers"):
print "Numbers"
time.sleep(5)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
self.ping = False
self.ping = False
while(self.run3):
totala = time.time() * 1000000
if(self.sub1 == "PingPong"):
if ((self.sub3 == "Ping_ghost") or (self.sub3 == "Pong_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub3 == "Ping"):
if(self.ping):
self.sub3 = "Pong"
self.ping = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Pong')
elif(self.sub3 == "Pong"):
if(self.pong):
self.sub3 = "Ping"
self.pong = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Ping')
# Actuation if
if(self.sub3 == "Ping"):
print " PING"
time.sleep(0.5)
self.ping= True
elif(self.sub3 == "Pong"):
print " PONG"
time.sleep(0.5)
self.pong = True
else:
if(self.sub3 == "Ping"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "Pong"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata4(self):
self.run4 = True
cycle = 100
t_activated = False
t_fin = 0
t_1_max = 0.001
t_2_max = 0.001
t_3_max = 0.001
while(self.run4):
totala = time.time() * 1000000
if(self.sub1 == "Numbers"):
if ((self.sub4 == "1_ghost") or (self.sub4 == "2_ghost") or (self.sub4 == "3_ghost")):
ghostStateIndex = self.StatesSub4.index(self.sub4)
self.sub4 = self.StatesSub4[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub4 == "1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_1_max):
self.sub4 = "2"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('2')
t_1_max = 0.001
elif(self.sub4 == "2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_2_max):
self.sub4 = "3"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('3')
t_2_max = 0.001
elif(self.sub4 == "3"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_3_max):
self.sub4 = "1"
t_activated = False
self.numbersFinished= True
if self.displayGui:
self.automataGui.notifySetNodeAsActive('1')
t_3_max = 0.001
# Actuation if
if(self.sub4 == "1"):
print " 1"
time.sleep(1)
elif(self.sub4 == "2"):
print " 2"
time.sleep(1)
elif(self.sub4 == "3"):
print " 3"
time.sleep(1)
else:
if(self.sub4 == "1"):
t_1_max = 0.001 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif(self.sub4 == "2"):
t_2_max = 0.001 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif(self.sub4 == "3"):
t_3_max = 0.001 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata5(self):
self.run5 = True
cycle = 100
t_activated = False
t_fin = 0
t_wait1_max = 1
t_wait2_max = 1
while(self.run5):
totala = time.time() * 1000000
if(self.sub3 == "Pong"):
if ((self.sub5 == "wait2_ghost") or (self.sub5 == "wait1_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub5 == "wait2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_wait2_max):
self.sub5 = "wait1"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('wait1')
t_wait2_max = 1
elif(self.sub5 == "wait1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_wait1_max):
self.sub5 = "wait2"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('wait2')
t_wait1_max = 1
# Actuation if
if(self.sub5 == "wait2"):
print ""
elif(self.sub5 == "wait1"):
print ""
else:
if(self.sub5 == "wait2"):
t_wait2_max = 1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "wait1"):
t_wait1_max = 1 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t4.join()
self.t5.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.StatesSub1 = [
"TakeOff",
"Stabilizing1",
"GoFront",
"Stoping",
"Landing",
"Stabilizing2",
]
self.sub1 = "TakeOff"
self.run1 = True
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = automatagui.GuiSubautomata(1,0)
guiSubautomata1.newGuiNode(1, 0, 61, 101, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 0, 283, 75, 0, 'Stabilizing1')
guiSubautomata1.newGuiNode(3, 0, 497, 130, 0, 'GoFront')
guiSubautomata1.newGuiNode(4, 0, 488, 320, 0, 'Stoping')
guiSubautomata1.newGuiNode(5, 0, 250, 408, 0, 'Landing')
guiSubautomata1.newGuiNode(6, 0, 66, 283, 0, 'Stabilizing2')
orig11 = automatagui.Point(61, 101)
dest11 = automatagui.Point(283, 75)
mid11 = automatagui.Point(38, 179)
guiSubautomata1.newGuiTransition(orig11, dest11, mid11, 1, 1, 2)
orig12 = automatagui.Point(283, 75)
dest12 = automatagui.Point(497, 130)
mid12 = automatagui.Point(413, 79)
guiSubautomata1.newGuiTransition(orig12, dest12, mid12, 2, 2, 3)
orig13 = automatagui.Point(497, 130)
dest13 = automatagui.Point(488, 320)
mid13 = automatagui.Point(570, 239)
guiSubautomata1.newGuiTransition(orig13, dest13, mid13, 3, 3, 4)
orig14 = automatagui.Point(488, 320)
dest14 = automatagui.Point(250, 408)
mid14 = automatagui.Point(410, 411)
guiSubautomata1.newGuiTransition(orig14, dest14, mid14, 4, 4, 5)
orig16 = automatagui.Point(250, 408)
dest16 = automatagui.Point(66, 283)
mid16 = automatagui.Point(152, 338)
guiSubautomata1.newGuiTransition(orig16, dest16, mid16, 6, 5, 6)
orig11 = automatagui.Point(66, 283)
dest11 = automatagui.Point(61, 101)
mid11 = automatagui.Point(38, 179)
guiSubautomata1.newGuiTransition(orig11, dest11, mid11, 1, 6, 1)
guiSubautomataList.append(guiSubautomata1);
return guiSubautomataList
def shutDown(self):
self.run1 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.automataGui.show()
app.exec_()
def subautomata1(self):
run = True
cycle = 100
t_activated = False
hasTakenOff = False
hasLanded = False
while(run):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "TakeOff"):
if(hasTakenOff):
self.sub1 = "Stabilizing1"
print "Going to Stabilizing"
elif(self.sub1 == "Stabilizing1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 1.5):
self.sub1 = "GoFront"
t_activated = False
print "going to GoFront"
elif(self.sub1 == "GoFront"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3):
self.sub1 = "Stoping"
t_activated = False
print "going to Stoping"
elif(self.sub1 == "Stoping"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3.5):
self.sub1 = "Landing"
t_activated = False
print "going to Land"
elif(self.sub1 == "Landing"):
if(hasLanded):
self.sub1 = "Stabilizing2"
elif(self.sub1 == "Stabilizing2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3):
self.sub1 = "TakeOff"
t_activated = False
print "Restarting"
# Actuation if
if(self.sub1 == "TakeOff"):
print "Taking Off"
self.lock.acquire()
#self.extraPrx.takeoff()
self.lock.release()
hasTakenOff = True
elif(self.sub1 == "Stabilizing1"):
print "Stabilizing"
elif(self.sub1 == "GoFront"):
cmd = jderobot.CMDVelData()
cmd.linearX = 1
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub1 == "Stoping"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdPrx.setCMDVelData(cmd)
self.lock.release()
print "Stoping"
elif(self.sub1 == "Landing"):
print "Landing"
self.lock.acquire()
#self.extraPrx.land()
self.lock.release()
hasLanded = True
elif(self.sub1 == "Stabilizing2"):
finished()
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to camera
camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if(not camera):
raise Exception('could not create proxy with camera')
self.cameraPrx = CameraPrx.checkedCast(camera)
if(not self.cameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'camera connected'
# Contact to pose3d
pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if(not pose3d):
raise Exception('could not create proxy with pose3d')
self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
if(not self.pose3dPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'pose3d connected'
# Contact to cmd
cmd = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if(not cmd):
raise Exception('could not create proxy with cmd')
self.cmdPrx = CMDVelPrx.checkedCast(cmd)
if(not self.cmdPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'cmd connected'
# Contact to extra
extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if(not extra):
raise Exception('could not create proxy with extra')
self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
if(not self.extraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'extra connected'
# Contact to navdata
navdata = self.ic.propertyToProxy('automata.Navdata.Proxy')
if(not navdata):
raise Exception('could not create proxy with navdata')
self.navdataPrx = NavdataPrx.checkedCast(navdata)
if(not self.navdataPrx):
raise Exception('invalid proxy automata.Navdata.Proxy')
print 'navdata connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
self.t1 = threading.Thread(target=self.subautomata1)
#self.t1.start()
def join(self):
self.guiThread.join()
self.t1.join()
class Automata(): def __init__(self): self.lock = threading.Lock() self.StatesSub1 = [ "GoFront", "GoBack", "Turn", "Wait", ] self.sub1 = "GoFront" self.run1 = True def shutDown(self): self.run1 = False def runGui(self): app = QtGui.QApplication(sys.argv) self.automataGui = AutomataGui() self.automataGui.setAutomata(createAutomata()) self.automataGui.loadAutomata() self.automataGui.show() app.exec_() def subautomata1(self): run = True cycle = 100 t_activated = False jderobot::EncodersDataPtr encoders = Encodersprx->getEncodersData(); float thetapos = 0; float angle = 0; while(run): totala = time.time() * 1000000 # Evaluation if if(self.sub1 == "GoFront"): if(not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if(secs > 2): self.sub1 = "GoBack" t_activated = False elif(self.sub1 == "GoBack"): if(not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if(secs > 2): self.sub1 = "Turn" t_activated = False elif(self.sub1 == "Turn"): if(angle > 45): self.sub1 = "Wait" elif(self.sub1 == "Wait"): if(not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if(secs > 1): self.sub1 = "GoFront" t_activated = False # Actuation if if(self.sub1 == "GoFront"): Motorsprx->setV(100.0); Motorsprx->setW(0.0); encoders = Encodersprx->getEncodersData(); thetapos = encoders->robottheta; elif(self.sub1 == "GoBack"): Motorsprx->setV(-100.0); Motorsprx->setW(0.0); encoders = Encodersprx->getEncodersData(); thetapos = encoders->robottheta; elif(self.sub1 == "Turn"): Motorsprx->setV(0.0); Motorsprx->setW(-3.0); encoders = Encodersprx->getEncodersData(); if (encoders->robottheta > thetapos + 1) angle = thetapos - encoders->robottheta + 360; else angle = thetapos - encoders->robottheta; std::cout << "angle: " << angle << std::endl; elif(self.sub1 == "Wait"): Motorsprx->setV(0.0); Motorsprx->setW(0.0); encoders = Encodersprx->getEncodersData(); thetapos = encoders->robottheta; totalb = time.time() * 1000000 msecs = (totalb - totala) / 1000; if(msecs < 0 or msecs > cycle): msecs = cycle else: msecs = cycle - msecs time.sleep(msecs / 1000) if(msecs < 33 ): time.sleep(33 / 1000);
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"State1",
"State2",
"State3",
"State4",
]
self.sub1 = "State1"
self.run1 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 80, 160, 1, 'State1')
guiSubautomata1.newGuiNode(2, 0, 473, 164, 0, 'State2')
guiSubautomata1.newGuiNode(3, 0, 475, 431, 0, 'State3')
guiSubautomata1.newGuiNode(4, 0, 94, 417, 0, 'State4')
guiSubautomata1.newGuiTransition((80, 160), (473, 164), (276, 162), 1, 1, 2)
guiSubautomata1.newGuiTransition((473, 164), (475, 431), (474, 298), 2, 2, 3)
guiSubautomata1.newGuiTransition((475, 431), (94, 417), (285, 424), 3, 3, 4)
guiSubautomata1.newGuiTransition((94, 417), (80, 160), (87, 288), 4, 4, 1)
guiSubautomataList.append(guiSubautomata1)
return guiSubautomataList
def shutDown(self):
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
self.finished = False
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "State1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 2):
self.sub1 = "State2"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('State2')
elif(self.sub1 == "State2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 2):
self.sub1 = "State3"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('State3')
elif(self.sub1 == "State3"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 2):
self.sub1 = "State4"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('State4')
elif(self.sub1 == "State4"):
if(self.finished):
self.sub1 = "State1"
print "Finished!"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('State1')
# Actuation if
if(self.sub1 == "State1"):
print "State1"
time.sleep(2)
elif(self.sub1 == "State2"):
print "State2"
time.sleep(2)
elif(self.sub1 == "State3"):
print "State3"
time.sleep(2)
elif(self.sub1 == "State4"):
print "State4"
time.sleep(2)
self.finished = True
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
if __name__ == '__main__':
signal.signal(signal.SIGINT, signal.SIG_DFL)
automata = Automata()
try:
automata.connectToProxys()
automata.readArgs()
automata.start()
automata.join()
sys.exit(0)
except:
traceback.print_exc()
automata.destroyIc()
sys.exit(-1)
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.StatesSub1 = [
"TakeOff",
"Stabilizing1",
"GoFront",
"Stoping",
"Landing",
"Stabilizing2",
]
self.StatesSub3 = [
"A",
"A_ghost",
"B",
"B_ghost",
]
self.StatesSub5 = [
"C",
"C_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub3 = "A_ghost"
self.run3 = True
self.sub5 = "C_ghost"
self.run5 = True
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 3, 61, 101, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 0, 283, 75, 0, 'Stabilizing1')
guiSubautomata1.newGuiNode(3, 0, 497, 130, 0, 'GoFront')
guiSubautomata1.newGuiNode(4, 0, 489, 320, 0, 'Stoping')
guiSubautomata1.newGuiNode(5, 0, 250, 408, 0, 'Landing')
guiSubautomata1.newGuiNode(6, 0, 66, 283, 0, 'Stabilizing2')
guiSubautomata1.newGuiTransition((61, 101), (283, 75), (9, 212), 1, 1,
2)
guiSubautomata1.newGuiTransition((283, 75), (497, 130), (413, 79), 2,
2, 3)
guiSubautomata1.newGuiTransition((497, 130), (489, 320), (570, 239), 3,
3, 4)
guiSubautomata1.newGuiTransition((489, 320), (250, 408), (410, 411), 4,
4, 5)
guiSubautomata1.newGuiTransition((250, 408), (66, 283), (152, 338), 6,
5, 6)
guiSubautomata1.newGuiTransition((66, 283), (61, 101), (9, 212), 1, 6,
1)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3, 1, self.automataGui)
guiSubautomata3.newGuiNode(7, 0, 175, 146, 1, 'A')
guiSubautomata3.newGuiNode(8, 5, 468, 201, 0, 'B')
guiSubautomata3.newGuiTransition((175, 146), (468, 201), (348, 79), 1,
7, 8)
guiSubautomata3.newGuiTransition((468, 201), (175, 146), (303, 215), 2,
8, 7)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5, 3, self.automataGui)
guiSubautomata5.newGuiNode(9, 0, 70, 146, 1, 'C')
guiSubautomata5.newGuiTransition((70, 146), (70, 146), (70, 186), 3, 9,
9)
guiSubautomataList.append(guiSubautomata5)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run5 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.automataGui.show()
app.exec_()
def subautomata1(self):
run = True
cycle = 100
t_activated = False
hasTakenOff = False
hasLanded = False
while (run):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "TakeOff"):
if (hasTakenOff):
self.sub1 = "Stabilizing1"
print "Going to Stabilizing"
self.automataGui.notifySetNodeAsActive('Stabilizing1')
elif (self.sub1 == "Stabilizing1"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 1.5):
self.sub1 = "GoFront"
t_activated = False
print "going to GoFront"
self.automataGui.notifySetNodeAsActive('GoFront')
elif (self.sub1 == "GoFront"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 3):
self.sub1 = "Stoping"
t_activated = False
print "going to Stoping"
self.automataGui.notifySetNodeAsActive('Stoping')
elif (self.sub1 == "Stoping"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 3.5):
self.sub1 = "Landing"
t_activated = False
print "going to Land"
self.automataGui.notifySetNodeAsActive('Landing')
elif (self.sub1 == "Landing"):
if (hasLanded):
self.sub1 = "Stabilizing2"
self.automataGui.notifySetNodeAsActive('Stabilizing2')
elif (self.sub1 == "Stabilizing2"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 3):
self.sub1 = "TakeOff"
t_activated = False
print "Restarting"
self.automataGui.notifySetNodeAsActive('TakeOff')
# Actuation if
if (self.sub1 == "TakeOff"):
print "Taking Off"
self.lock.acquire()
self.extraPrx.takeoff()
self.lock.release()
hasTakenOff = True
elif (self.sub1 == "Stabilizing1"):
print "Stabilizing"
elif (self.sub1 == "GoFront"):
cmd = jderobot.CMDVelData()
cmd.linearX = 1
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdPrx.setCMDVelData(cmd)
self.lock.release()
elif (self.sub1 == "Stoping"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdPrx.setCMDVelData(cmd)
self.lock.release()
print "Stoping"
elif (self.sub1 == "Landing"):
print "Landing"
self.lock.acquire()
self.extraPrx.land()
self.lock.release()
hasLanded = True
elif (self.sub1 == "Stabilizing2"):
finished()
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata3(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_A_max = 0.023
t_B_max = 0.055
while (run):
totala = time.time() * 1000000
if (self.sub1 == "TakeOff"):
if ((self.sub3 == "A_ghost") or (self.sub3 == "B_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub3 == "A"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_A_max):
self.sub3 = "B"
t_activated = False
self.automataGui.notifySetNodeAsActive('B')
t_A_max = 0.023
elif (self.sub3 == "B"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_B_max):
self.sub3 = "A"
t_activated = False
self.automataGui.notifySetNodeAsActive('A')
t_B_max = 0.055
# Actuation if
if (self.sub3 == "A"):
print "A"
elif (self.sub3 == "B"):
print "B"
else:
if (self.sub3 == "A"):
t_A_max = 0.023 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
sub3 = self.StatesSub3[ghostStateIndex]
elif (self.sub3 == "B"):
t_B_max = 0.055 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata5(self):
run = True
cycle = 100
t_activated = False
t_C_max = 0.567
while (run):
totala = time.time() * 1000000
if (self.sub3 == "B"):
if ((self.sub5 == "C_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub5 == "C"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > t_C_max):
self.sub5 = "C"
t_activated = False
self.automataGui.notifySetNodeAsActive('C')
t_C_max = 0.567
# Actuation if
if (self.sub5 == "C"):
print "C"
else:
if (self.sub5 == "C"):
t_C_max = 0.567 - (t_fin - t_ini)
ghostStateIndex = self.StateSub5.index(self.sub5) + 1
sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to camera
camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if (not camera):
raise Exception('could not create proxy with camera')
self.cameraPrx = CameraPrx.checkedCast(camera)
if (not self.cameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'camera connected'
# Contact to pose3d
pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if (not pose3d):
raise Exception('could not create proxy with pose3d')
self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
if (not self.pose3dPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'pose3d connected'
# Contact to cmd
cmd = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if (not cmd):
raise Exception('could not create proxy with cmd')
self.cmdPrx = CMDVelPrx.checkedCast(cmd)
if (not self.cmdPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'cmd connected'
# Contact to extra
extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if (not extra):
raise Exception('could not create proxy with extra')
self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
if (not self.extraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'extra connected'
# Contact to navdata
navdata = self.ic.propertyToProxy('automata.Navdata.Proxy')
if (not navdata):
raise Exception('could not create proxy with navdata')
self.navdataPrx = NavdataPrx.checkedCast(navdata)
if (not self.navdataPrx):
raise Exception('invalid proxy automata.Navdata.Proxy')
print 'navdata connected'
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
time.sleep(1)
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
def join(self):
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t5.join()
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"Go",
"GoBack",
"Rotate",
]
self.sub1 = "Go"
self.run1 = True
def getRobotTheta(self):
d = self.EncodersPrx.getPose3DData()
theta = math.atan2(2*(d.q0*d.q3 + d.q1*d.q2), 1-2*(d.q2*d.q2 + d.q3*d.q3))
return theta
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 112, 126, 1, 'Go')
guiSubautomata1.newGuiNode(2, 0, 372, 136, 0, 'GoBack')
guiSubautomata1.newGuiNode(3, 0, 238, 308, 0, 'Rotate')
guiSubautomata1.newGuiTransition((112, 126), (372, 136), (237, 114), 1, 1, 2)
guiSubautomata1.newGuiTransition((372, 136), (238, 308), (305, 222), 2, 2, 3)
guiSubautomata1.newGuiTransition((238, 308), (112, 126), (140, 216), 3, 3, 1)
guiSubautomataList.append(guiSubautomata1)
return guiSubautomataList
def shutDown(self):
self.run1 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
laserData = self.LasersPrx.getLaserData()
minDistance = 1.5
dist = None
destinyAngle = None
error = 0
print "numero de lasers (grados):", laserData.numLaser
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "Go"):
if(dist != None and dist < minDistance):
self.sub1 = "GoBack"
self.MotorsPrx.setV(0)
print "stopping"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoBack')
elif(self.sub1 == "GoBack"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 1.5):
self.sub1 = "Rotate"
t_activated = False
angle = self.getRobotTheta()
print "my Angle:", angle
self.MotorsPrx.setV(0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Rotate')
elif(self.sub1 == "Rotate"):
if(error <= 0.1 and error >= -0.1):
self.sub1 = "Go"
destinyAngle =None
self.MotorsPrx.setW(0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Go')
# Actuation if
if(self.sub1 == "Go"):
self.MotorsPrx.setV(1)
laserData = self.LasersPrx.getLaserData()
dist = None
for i in range(60, 120):
dist = laserData.distanceData[i]/1000
if dist == None or laserData.distanceData[i]/1000 < dist:
dist = laserData.distanceData[i]/1000
print "dist:", dist
elif(self.sub1 == "GoBack"):
self.MotorsPrx.setV(-0.1)
print "back"
elif(self.sub1 == "Rotate"):
if destinyAngle == None:
turn = random.uniform(-math.pi, math.pi)
destinyAngle = (angle + turn) % (math.pi)
print "DestinyAngle:", destinyAngle
angle = self.getRobotTheta()
error = (destinyAngle - angle)*0.75
if error > 0 and error < 0.1:
error = 0.1
elif error < 0 and error > -0.1:
error = -0.1
print "angle:", angle, "destiny:", destinyAngle, "speed:", error
self.MotorsPrx.setW(error)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Motors
Motors = self.ic.propertyToProxy('automata.Motors.Proxy')
if(not Motors):
raise Exception('could not create proxy with Motors')
self.MotorsPrx = MotorsPrx.checkedCast(Motors)
if(not self.MotorsPrx):
raise Exception('invalid proxy automata.Motors.Proxy')
print 'Motors connected'
# Contact to Pose3D
Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if(not Pose3D):
raise Exception('could not create proxy with Pose3D')
self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
if(not self.Pose3DPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'Pose3D connected'
# Contact to Laser
Laser = self.ic.propertyToProxy('automata.Laser.Proxy')
if(not Laser):
raise Exception('could not create proxy with Laser')
self.LaserPrx = LaserPrx.checkedCast(Laser)
if(not self.LaserPrx):
raise Exception('invalid proxy automata.Laser.Proxy')
print 'Laser connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"square",
"wait",
]
self.StatesSub2 = [
"go_up",
"go_up_ghost",
"turn_rigth",
"turn_rigth_ghost",
"go_rigth",
"go_rigth_ghost",
"turn_down",
"turn_down_ghost",
"go_down",
"go_down_ghost",
"turn_left",
"turn_left_ghost",
"go_left",
"go_left_ghost",
"turn_up",
"turn_up_ghost",
]
self.StatesSub3 = [
"wait1",
"wait1_ghost",
"wait2",
"wait2_ghost",
]
self.sub1 = "square"
self.run1 = True
self.sub2 = "go_up_ghost"
self.run2 = True
self.sub3 = "wait1_ghost"
self.run3 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t2 = threading.Thread(target=self.subautomata2)
self.t2.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 2, 134, 346, 1, 'square')
guiSubautomata1.newGuiNode(2, 3, 534, 349, 0, 'wait')
guiSubautomata1.newGuiTransition((134, 346), (534, 349), (337, 230), 1, 1, 2)
guiSubautomata1.newGuiTransition((534, 349), (134, 346), (335, 457), 4, 2, 1)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata2
guiSubautomata2 = GuiSubautomata(2,1, self.automataGui)
guiSubautomata2.newGuiNode(5, 0, 168, 559, 1, 'go_up')
guiSubautomata2.newGuiNode(8, 0, 69, 396, 0, 'turn_rigth')
guiSubautomata2.newGuiNode(9, 0, 185, 270, 0, 'go_rigth')
guiSubautomata2.newGuiNode(10, 0, 343, 192, 0, 'turn_down')
guiSubautomata2.newGuiNode(11, 0, 486, 283, 0, 'go_down')
guiSubautomata2.newGuiNode(12, 0, 555, 426, 0, 'turn_left')
guiSubautomata2.newGuiNode(13, 0, 505, 583, 0, 'go_left')
guiSubautomata2.newGuiNode(14, 0, 354, 650, 0, 'turn_up')
guiSubautomata2.newGuiTransition((168, 559), (69, 396), (118, 477), 10, 5, 8)
guiSubautomata2.newGuiTransition((69, 396), (185, 270), (127, 333), 12, 8, 9)
guiSubautomata2.newGuiTransition((185, 270), (343, 192), (264, 231), 13, 9, 10)
guiSubautomata2.newGuiTransition((343, 192), (486, 283), (415, 237), 14, 10, 11)
guiSubautomata2.newGuiTransition((486, 283), (555, 426), (525, 348), 15, 11, 12)
guiSubautomata2.newGuiTransition((555, 426), (505, 583), (534, 498), 16, 12, 13)
guiSubautomata2.newGuiTransition((505, 583), (354, 650), (430, 616), 17, 13, 14)
guiSubautomata2.newGuiTransition((354, 650), (168, 559), (261, 604), 18, 14, 5)
guiSubautomataList.append(guiSubautomata2)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3,2, self.automataGui)
guiSubautomata3.newGuiNode(15, 0, 166, 223, 1, 'wait1')
guiSubautomata3.newGuiNode(16, 0, 446, 237, 0, 'wait2')
guiSubautomata3.newGuiTransition((166, 223), (446, 237), (291, 127), 1, 15, 16)
guiSubautomata3.newGuiTransition((446, 237), (166, 223), (291, 301), 3, 16, 15)
guiSubautomataList.append(guiSubautomata3)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run2 = False
self.run3 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
while(run):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "square"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 20):
self.sub1 = "wait"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('wait')
elif(self.sub1 == "wait"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 5):
self.sub1 = "square"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('square')
# Actuation if
if(self.sub1 == "wait"):
Motorsprx->setV(0.0);
Motorsprx->setW(0.0);
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata2(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_go_up_max = 2
t_go_rigth_max = 2
t_go_down_max = 2
t_go_left_max = 2
jderobot::EncodersDataPtr encoders = Encodersprx->getEncodersData();
float thetapos = 0;
float angle = 0;
while(run):
totala = time.time() * 1000000
if(self.sub1 == "square"):
if ((self.sub2 == "go_up_ghost") or (self.sub2 == "turn_rigth_ghost") or (self.sub2 == "go_rigth_ghost") or (self.sub2 == "turn_down_ghost") or (self.sub2 == "go_down_ghost") or (self.sub2 == "turn_left_ghost") or (self.sub2 == "go_left_ghost") or (self.sub2 == "turn_up_ghost")):
ghostStateIndex = self.StatesSub2.index(self.sub2)
self.sub2 = self.StatesSub2[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub2 == "go_up"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_go_up_max):
self.sub2 = "turn_rigth"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('turn_rigth')
t_go_up_max = 2
elif(self.sub2 == "turn_rigth"):
if(angle > 90):
self.sub2 = "go_rigth"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('go_rigth')
elif(self.sub2 == "go_rigth"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_go_rigth_max):
self.sub2 = "turn_down"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('turn_down')
t_go_rigth_max = 2
elif(self.sub2 == "turn_down"):
if(angle > 90):
self.sub2 = "go_down"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('go_down')
elif(self.sub2 == "go_down"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_go_down_max):
self.sub2 = "turn_left"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('turn_left')
t_go_down_max = 2
elif(self.sub2 == "turn_left"):
if(angle > 90):
self.sub2 = "go_left"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('go_left')
elif(self.sub2 == "go_left"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_go_left_max):
self.sub2 = "turn_up"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('turn_up')
t_go_left_max = 2
elif(self.sub2 == "turn_up"):
if(angle > 90):
self.sub2 = "go_up"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('go_up')
# Actuation if
if(self.sub2 == "go_up"):
Motorsprx->setV(100.0);
Motorsprx->setW(0.0);
encoders = Encodersprx->getEncodersData();
thetapos = encoders->robottheta;
elif(self.sub2 == "turn_rigth"):
Motorsprx->setV(0.0);
Motorsprx->setW(-3.0);
encoders = Encodersprx->getEncodersData();
if (encoders->robottheta > thetapos + 1)
angle = thetapos - encoders->robottheta + 360;
else
angle = thetapos - encoders->robottheta;
std::cout << "angle: " << angle << std::endl;
elif(self.sub2 == "go_rigth"):
Motorsprx->setV(100.0);
Motorsprx->setW(0.0);
encoders = Encodersprx->getEncodersData();
thetapos = encoders->robottheta;
elif(self.sub2 == "turn_down"):
Motorsprx->setV(0.0);
Motorsprx->setW(-3.0);
encoders = Encodersprx->getEncodersData();
if (encoders->robottheta > thetapos + 1)
angle = thetapos - encoders->robottheta + 360;
else
angle = thetapos - encoders->robottheta;
std::cout << "angle: " << angle << std::endl;
elif(self.sub2 == "go_down"):
Motorsprx->setV(100.0);
Motorsprx->setW(0.0);
encoders = Encodersprx->getEncodersData();
thetapos = encoders->robottheta;
elif(self.sub2 == "turn_left"):
Motorsprx->setV(0.0);
Motorsprx->setW(-3.0);
encoders = Encodersprx->getEncodersData();
if (encoders->robottheta > thetapos + 1)
angle = thetapos - encoders->robottheta + 360;
def runGui(self): app = QtGui.QApplication(sys.argv) self.automataGui = AutomataGui() self.automataGui.draw() self.automataGui.show() app.exec_()
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"GoForward",
"GoBack",
]
self.sub1 = "GoForward"
self.run1 = True
def calculate_obstacle(self):
self.laserData = self.KobukiLaser.getLaserData()
min_dist = 1000
for i in range(len(self.laserData.values)):
if self.laserData.values[i] < min_dist:
min_dist = self.laserData.values[i]
if min_dist < 1.0:
return True
else:
return False
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 69, 163, 1, 'GoForward')
guiSubautomata1.newGuiNode(2, 0, 255, 117, 0, 'GoBack')
guiSubautomata1.newGuiTransition((69, 163), (255, 117), (139, 78), 1, 1, 2)
guiSubautomata1.newGuiTransition((255, 117), (69, 163), (189, 196), 2, 2, 1)
guiSubautomataList.append(guiSubautomata1)
return guiSubautomataList
def shutDown(self):
self.run1 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "GoForward"):
if(self.calculate_obstacle()):
self.sub1 = "GoBack"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoBack')
elif(self.sub1 == "GoBack"):
if(not self.calculate_obstacle()):
self.sub1 = "GoForward"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoForward')
# Actuation if
if(self.sub1 == "GoForward"):
self.KobukiMotors.sendV(0.5)
self.KobukiMotors.sendW(0.0)
elif(self.sub1 == "GoBack"):
self.KobukiMotors.sendV(-0.3)
self.KobukiMotors.sendW(0.2)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = EasyIce.initialize(sys.argv)
self.ic,self.node = comm.init(self.ic)
# Contact to KobukiMotors
self.KobukiMotors = comm.getMotorsClient(self.ic, 'automata.KobukiMotors')
if(not self.KobukiMotors):
raise Exception('could not create client with KobukiMotors')
print('KobukiMotors connected')
# Contact to KobukiLaser
self.KobukiLaser = comm.getLaserClient(self.ic, 'automata.KobukiLaser')
if(not self.KobukiLaser):
raise Exception('could not create client with KobukiLaser')
print('KobukiLaser connected')
def destroyIc(self):
self.KobukiMotors.stop()
self.KobukiLaser.stop()
comm.destroy(self.ic, self.node)
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print('runtime gui enabled')
else:
self.displayGui = False
print('runtime gui disabled')
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"FollowTurtle",
"Land",
]
self.StatesSub2 = [
"FindTurtle",
"FindTurtle_ghost",
"FollowTurtle",
"FollowTurtle_ghost",
]
self.StatesSub3 = [
"LoseTurtle",
"LoseTurtle_ghost",
"Landing",
"Landing_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub2 = "FindTurtle_ghost"
self.run2 = True
self.sub3 = "LoseTurtle_ghost"
self.run3 = True
def setVelocity(self, vx, vy, vz, yaw):
cmd = jderobot.CMDVelData()
cmd.linearX = vy
cmd.linearY = vx
cmd.linearZ = vz
cmd.angularZ = yaw
cmd.angularX = cmd.angularY = 1.0
self.CMDVelPrx.setCMDVelData(cmd)
def getImage(self):
img = self.CameraPrx.getImageData("RGB8")
height = img.description.height
width = img.description.width
imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
imgPixels.shape = height, width, 3
return imgPixels
def getContours(self):
img = self.getImage()
img = cv2.GaussianBlur(img, (5, 5), 0)
hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
maxValues = numpy.array(self.hmax, self.vmax, self.smax)
minValues = numpy.array(self.hmin, self.vmin, self.smin)
thresoldImg = cv2.inRange(hsvImg, minValues, maxValues)
conts, hierarchy = cv2.findContours(thresoldImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
return conts, hierarchy
class PID:
def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
self.Ep = Epsilon
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.Imax = Imax
self.Imin = Imin
self.lastError = 0
self.cumulativeError = 0
def updateCumulativeError(self, error):
self.cumulativeError += error
if self.cumulativeError > self.Imax:
self.cumulativeError = self.Imax
if self.cumulativeError < self.Imin:
self.cumulativeError = self.Imin
def process(self, error):
if -self.Ep < error < self.Ep:
return 0
P = self.Kp * error
self.updateCumulativeError(error)
I = self.Ki * self.cumulativeError
D = self.Kd * (error - self.lastError)
self.lastError = error
vel = P + I + D
return vel
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t2 = threading.Thread(target=self.subautomata2)
self.t2.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 111, 195, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 2, 323, 206, 0, 'FollowTurtle')
guiSubautomata1.newGuiNode(3, 3, 531, 229, 0, 'Land')
guiSubautomata1.newGuiTransition((111, 195), (323, 206), (191, 244), 1, 1, 2)
guiSubautomata1.newGuiTransition((323, 206), (531, 229), (419, 267), 2, 2, 3)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata2
guiSubautomata2 = GuiSubautomata(2,2, self.automataGui)
guiSubautomata2.newGuiNode(4, 0, 118, 156, 1, 'FindTurtle')
guiSubautomata2.newGuiNode(5, 0, 410, 171, 0, 'FollowTurtle')
guiSubautomata2.newGuiTransition((118, 156), (410, 171), (276, 101), 3, 4, 5)
guiSubautomata2.newGuiTransition((410, 171), (118, 156), (258, 221), 4, 5, 4)
guiSubautomataList.append(guiSubautomata2)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3,3, self.automataGui)
guiSubautomata3.newGuiNode(7, 0, 117, 188, 1, 'LoseTurtle')
guiSubautomata3.newGuiNode(8, 0, 378, 213, 0, 'Landing')
guiSubautomata3.newGuiTransition((378, 213), (117, 188), (271, 109), 6, 8, 7)
guiSubautomata3.newGuiTransition((117, 188), (378, 213), (250, 258), 7, 7, 8)
guiSubautomataList.append(guiSubautomata3)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run2 = False
self.run3 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
self.heighIters = 0
self.maxIters = 50
self.contours = []
#Filter Values
self.hmin = 50
self.hmax = 70
self.vmin = 20
self.vmax = 235
self.smin = 10
self.smax = 245
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "TakeOff"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 2.5):
self.sub1 = "FollowTurtle"
t_activated = False
print "Iters:", self.heighIters
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowTurtle')
elif(self.sub1 == "FollowTurtle"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 90):
self.sub1 = "Land"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Land')
# Actuation if
if(self.sub1 == "TakeOff"):
self.ExtraPrx.takeoff()
#Get some heigh
self.setVelocity(0, 0, 1, 0)
self.heighIters += 1
elif(self.sub1 == "FollowTurtle"):
self.contours, hierarchy = self.getContours()
elif(self.sub1 == "Land"):
self.contours, hierarchy = self.getContours()
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata2(self):
self.run2 = True
cycle = 100
t_activated = False
t_fin = 0
targetX = 125
targetY = 125
minError = 8
xPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)
yPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)
while(self.run2):
totala = time.time() * 1000000
if(self.sub1 == "FollowTurtle"):
if ((self.sub2 == "FindTurtle_ghost") or (self.sub2 == "FollowTurtle_ghost")):
ghostStateIndex = self.StatesSub2.index(self.sub2)
self.sub2 = self.StatesSub2[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub2 == "FindTurtle"):
if(self.contours):
self.sub2 = "FollowTurtle"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowTurtle')
elif(self.sub2 == "FollowTurtle"):
if(not self.contours):
self.sub2 = "FindTurtle"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FindTurtle')
# Actuation if
if(self.sub2 == "FindTurtle"):
if self.heighIters <= self.maxIters:
self.setVelocity(0, 0, 1, 0)
self.heighIters += 1
print "iters:", self.heighIters
else:
print "max hight reached"
elif(self.sub2 == "FollowTurtle"):
#We assume there is only one green target on the screen
cnt = self.contours[0]
#Locate the minimal circle enclosing the contour
(x, y), radius = cv2.minEnclosingCircle(cnt)
center = (int(x), int(y))
radius = int(radius)
xError = targetX - center[0]
yError = targetY - center[1]
xVel = xPid.process(xError)
yVel = yPidprocess(yError)
#Control Heigh#
self.setVelocity(xVel, yVel, 0, 0)
print "xError:", xError, "yError:", yError
print "xVel:", xVel, "yVel:", yVel
else:
if(self.sub2 == "FindTurtle"):
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
elif(self.sub2 == "FollowTurtle"):
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run3):
totala = time.time() * 1000000
if(self.sub1 == "Land"):
if ((self.sub3 == "LoseTurtle_ghost") or (self.sub3 == "Landing_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub3 == "LoseTurtle"):
if(not self.contours):
self.sub3 = "Landing"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Landing')
elif(self.sub3 == "Landing"):
if(self.contours):
self.sub3 = "LoseTurtle"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('LoseTurtle')
# Actuation if
if(self.sub3 == "LoseTurtle"):
self.setVelocity(1, 0, 0, 0)
elif(self.sub3 == "Landing"):
self.ExtraPrx.land()
else:
if(self.sub3 == "LoseTurtle"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "Landing"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Extra
Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if(not Extra):
raise Exception('could not create proxy with Extra')
self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
if(not self.ExtraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'Extra connected'
# Contact to CMDVel
CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if(not CMDVel):
raise Exception('could not create proxy with CMDVel')
self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
if(not self.CMDVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'CMDVel connected'
# Contact to Camera
Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if(not Camera):
raise Exception('could not create proxy with Camera')
self.CameraPrx = CameraPrx.checkedCast(Camera)
if(not self.CameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'Camera connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t2.join()
self.t3.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"Go",
"GoBack",
"Rotate",
]
self.sub1 = "Go"
self.run1 = True
def getRobotTheta(self):
d = self.EncodersPrx.getPose3DData()
theta = math.atan2(2 * (d.q0 * d.q3 + d.q1 * d.q2),
1 - 2 * (d.q2 * d.q2 + d.q3 * d.q3))
return theta
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 112, 126, 1, 'Go')
guiSubautomata1.newGuiNode(2, 0, 372, 136, 0, 'GoBack')
guiSubautomata1.newGuiNode(3, 0, 238, 308, 0, 'Rotate')
guiSubautomata1.newGuiTransition((112, 126), (372, 136), (237, 114), 1,
1, 2)
guiSubautomata1.newGuiTransition((372, 136), (238, 308), (305, 222), 2,
2, 3)
guiSubautomata1.newGuiTransition((238, 308), (112, 126), (140, 216), 3,
3, 1)
guiSubautomataList.append(guiSubautomata1)
return guiSubautomataList
def shutDown(self):
self.run1 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
laserData = self.LasersPrx.getLaserData()
minDistance = 1.5
dist = None
destinyAngle = None
error = 0
print "numero de lasers (grados):", laserData.numLaser
while (self.run1):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "Go"):
if (dist != None and dist < minDistance):
self.sub1 = "GoBack"
self.MotorsPrx.setV(0)
print "stopping"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('GoBack')
elif (self.sub1 == "GoBack"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 1.5):
self.sub1 = "Rotate"
t_activated = False
angle = self.getRobotTheta()
print "my Angle:", angle
self.MotorsPrx.setV(0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Rotate')
elif (self.sub1 == "Rotate"):
if (error <= 0.1 and error >= -0.1):
self.sub1 = "Go"
destinyAngle = None
self.MotorsPrx.setW(0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Go')
# Actuation if
if (self.sub1 == "Go"):
self.MotorsPrx.setV(1)
laserData = self.LasersPrx.getLaserData()
dist = None
for i in range(60, 120):
dist = laserData.distanceData[i] / 1000
if dist == None or laserData.distanceData[i] / 1000 < dist:
dist = laserData.distanceData[i] / 1000
print "dist:", dist
elif (self.sub1 == "GoBack"):
self.MotorsPrx.setV(-0.1)
print "back"
elif (self.sub1 == "Rotate"):
if destinyAngle == None:
turn = random.uniform(-math.pi, math.pi)
destinyAngle = (angle + turn) % (math.pi)
print "DestinyAngle:", destinyAngle
angle = self.getRobotTheta()
error = (destinyAngle - angle) * 0.75
if error > 0 and error < 0.1:
error = 0.1
elif error < 0 and error > -0.1:
error = -0.1
print "angle:", angle, "destiny:", destinyAngle, "speed:", error
self.MotorsPrx.setW(error)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Motors
Motors = self.ic.propertyToProxy('automata.Motors.Proxy')
if (not Motors):
raise Exception('could not create proxy with Motors')
self.MotorsPrx = MotorsPrx.checkedCast(Motors)
if (not self.MotorsPrx):
raise Exception('invalid proxy automata.Motors.Proxy')
print 'Motors connected'
# Contact to Pose3D
Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if (not Pose3D):
raise Exception('could not create proxy with Pose3D')
self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
if (not self.Pose3DPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'Pose3D connected'
# Contact to Laser
Laser = self.ic.propertyToProxy('automata.Laser.Proxy')
if (not Laser):
raise Exception('could not create proxy with Laser')
self.LaserPrx = LaserPrx.checkedCast(Laser)
if (not self.LaserPrx):
raise Exception('invalid proxy automata.Laser.Proxy')
print 'Laser connected'
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"goforward",
]
self.sub1 = "goforward"
self.run1 = True
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 106, 166, 1, 'goforward')
guiSubautomataList.append(guiSubautomata1)
return guiSubautomataList
def shutDown(self):
self.run1 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run1):
totala = time.time() * 1000000
# Evaluation if
# Actuation if
if (self.sub1 == "goforward"):
self.my_motors.sendV(0.2)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = EasyIce.initialize(sys.argv)
self.ic, self.node = comm.init(self.ic)
# Contact to my_motors
self.my_motors = comm.getMotorsClient(self.ic, 'automata.my_motors')
if (not self.my_motors):
raise Exception('could not create client with my_motors')
print('my_motors connected')
def destroyIc(self):
self.my_motors.stop()
comm.destroy(self.ic, self.node)
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print('runtime gui enabled')
else:
self.displayGui = False
print('runtime gui disabled')
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"FollowRoad",
"MonitorArea",
"TurnAround",
"Landing",
"END",
"HeighControl",
]
self.StatesSub3 = [
"FindRoad",
"FindRoad_ghost",
"FollowingRoad",
"FollowingRoad_ghost",
]
self.StatesSub5 = [
"ToFirstPos",
"ToFirstPos_ghost",
"ToSecondPos",
"ToSecondPos_ghost",
"ToThirdPos",
"ToThirdPos_ghost",
"Return",
"Return_ghost",
]
self.StatesSub6 = [
"Descending",
"Descending_ghost",
"Land",
"Land_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub3 = "FindRoad_ghost"
self.run3 = True
self.sub5 = "ToFirstPos_ghost"
self.run5 = True
self.sub6 = "Descending_ghost"
self.run6 = True
def getImage(self):
img = self.CameraPrx.getImageData("RGB8")
height = img.description.height
width = img.description.width
imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
imgPixels.shape = height, width, 3
return imgPixels
def setVelocity(self, vx, vy, vz, yaw):
cmd = jderobot.CMDVelData()
cmd.linearX = vy
cmd.linearY = vx
cmd.linearZ = vz
cmd.angularZ = yaw
cmd.angularX = cmd.angularY = 1.0
self.CMDVelPrx.setCMDVelData(cmd)
#The factor indicate the margin of the error multipliyin the error for this factor
def droneInPosition(self, pos, factor=1):
return (abs(pos[0] - self.xPos) < self.minDist * factor) and (
abs(pos[1] - self.yPos) < self.minDist * factor)
class PID:
def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
self.Ep = Epsilon
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.Imax = Imax
self.Imin = Imin
self.lastError = 0
self.cumulativeError = 0
def updateCumulativeError(self, error):
self.cumulativeError += error
if self.cumulativeError > self.Imax:
self.cumulativeError = self.Imax
elif self.cumulativeError < self.Imin:
self.cumulativeError = self.Imin
def process(self, error, derivative=None, integral=None):
if -self.Ep < error < self.Ep:
return 0
P = self.Kp * error
self.updateCumulativeError(error)
if integral != None:
I = self.Ki * integral
else:
I = self.Ki * self.cumulativeError
if derivative != None:
D = self.Kd * derivative
else:
D = self.Kd * (error - self.lastError)
self.lastError = error
speed = P + I + D
if speed > 3:
speed = 3
elif speed < -3:
speed = -3
return speed
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
self.t6 = threading.Thread(target=self.subautomata6)
self.t6.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 62, 66, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 3, 189, 116, 0, 'FollowRoad')
guiSubautomata1.newGuiNode(6, 5, 309, 268, 0, 'MonitorArea')
guiSubautomata1.newGuiNode(11, 0, 263, 428, 0, 'TurnAround')
guiSubautomata1.newGuiNode(12, 6, 53, 239, 0, 'Landing')
guiSubautomata1.newGuiNode(14, 0, 41, 427, 0, 'END')
guiSubautomata1.newGuiNode(17, 0, 481, 139, 0, 'HeighControl')
guiSubautomata1.newGuiTransition((189, 116), (309, 268), (274, 172), 7,
2, 6)
guiSubautomata1.newGuiTransition((309, 268), (263, 428), (349, 362),
13, 6, 11)
guiSubautomata1.newGuiTransition((263, 428), (189, 116), (164, 318),
14, 11, 2)
guiSubautomata1.newGuiTransition((189, 116), (53, 239), (82, 154), 19,
2, 12)
guiSubautomata1.newGuiTransition((53, 239), (41, 427), (43, 326), 22,
12, 14)
guiSubautomata1.newGuiTransition((481, 139), (189, 116), (328, 116),
26, 17, 2)
guiSubautomata1.newGuiTransition((62, 66), (481, 139), (430, 27), 27,
1, 17)
guiSubautomata1.newGuiTransition((189, 116), (481, 139), (315, 70), 28,
2, 17)
guiSubautomata1.newGuiTransition((309, 268), (481, 139), (378, 195),
29, 6, 17)
guiSubautomata1.newGuiTransition((481, 139), (309, 268), (412, 279),
30, 17, 6)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3, 2, self.automataGui)
guiSubautomata3.newGuiNode(3, 0, 156, 228, 1, 'FindRoad')
guiSubautomata3.newGuiNode(4, 0, 427, 255, 0, 'FollowingRoad')
guiSubautomata3.newGuiTransition((156, 228), (427, 255), (297, 157), 2,
3, 4)
guiSubautomata3.newGuiTransition((427, 255), (156, 228), (265, 320), 3,
4, 3)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5, 6, self.automataGui)
guiSubautomata5.newGuiNode(7, 0, 86, 275, 1, 'ToFirstPos')
guiSubautomata5.newGuiNode(8, 0, 247, 92, 0, 'ToSecondPos')
guiSubautomata5.newGuiNode(9, 0, 491, 303, 0, 'ToThirdPos')
guiSubautomata5.newGuiNode(10, 0, 281, 455, 0, 'Return')
guiSubautomata5.newGuiTransition((86, 275), (247, 92), (166, 184), 9,
7, 8)
guiSubautomata5.newGuiTransition((247, 92), (491, 303), (369, 198), 10,
8, 9)
guiSubautomata5.newGuiTransition((491, 303), (281, 455), (386, 379),
11, 9, 10)
guiSubautomata5.newGuiTransition((281, 455), (86, 275), (184, 365), 12,
10, 7)
guiSubautomataList.append(guiSubautomata5)
# Creating subAutomata6
guiSubautomata6 = GuiSubautomata(6, 12, self.automataGui)
guiSubautomata6.newGuiNode(15, 0, 126, 185, 1, 'Descending')
guiSubautomata6.newGuiNode(16, 0, 350, 190, 0, 'Land')
guiSubautomata6.newGuiTransition((126, 185), (350, 190), (232, 220),
24, 15, 16)
guiSubautomataList.append(guiSubautomata6)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run5 = False
self.run6 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
self.targetX = 125
self.targetY = 125
self.targetZ = 2.5
self.initPos = (-1, -8.5)
landingPose = (self.Pose3DPrx.getPose3DData().x,
self.Pose3DPrx.getPose3DData().y)
#Minimun errors
self.minError = 10
self.minActit = 0.5
self.minAltit = 0.01
self.minDist = 0.01
heighMargin = 1
#Control Variables
self.heigh = 0
self.contours = []
self.monitorComplet = False
self.xPos = landingPose[0]
self.yPos = landingPose[1]
initAngle = None
self.hasLanded = False
currentState = "FollowRoad"
takenOff = False
#Filter Values
self.hmin = 90
self.hmax = 97
self.vmin = 0
self.vmax = 50
self.smin = 45
self.smax = 80
#Control PID
self.xPid = self.PID(Epsilon=self.minError,
Kp=0.01,
Ki=0.04,
Kd=0.003,
Imax=5,
Imin=-5)
self.zPid = self.PID(Epsilon=self.minAltit,
Kp=1,
Ki=0.02,
Kd=0,
Imax=5,
Imin=-5)
self.aPid = self.PID(Epsilon=self.minActit,
Kp=0.01,
Ki=0.04,
Kd=0.003,
Imax=5,
Imin=-5)
while (self.run1):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "TakeOff"):
if (takenOff):
self.sub1 = "HeighControl"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('HeighControl')
elif (self.sub1 == "FollowRoad"):
if ((self.droneInPosition(self.initPos, 200))
and (not self.monitorComplet)):
self.sub1 = "MonitorArea"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('MonitorArea')
elif ((self.droneInPosition(landingPose, 75))
and (self.monitorComplet)):
self.sub1 = "Landing"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Landing')
elif (abs(self.targetZ - self.heigh) > heighMargin):
self.sub1 = "HeighControl"
currentState = "FollowRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('HeighControl')
elif (self.sub1 == "MonitorArea"):
if (self.monitorComplet):
self.sub1 = "TurnAround"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('TurnAround')
elif (abs(self.targetZ - self.heigh) > heighMargin):
self.sub1 = "HeighControl"
currentState = "MonitorArea"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('HeighControl')
elif (self.sub1 == "TurnAround"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 2.5):
self.sub1 = "FollowRoad"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowRoad')
elif (self.sub1 == "Landing"):
if (self.hasLanded):
self.sub1 = "END"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('END')
elif (self.sub1 == "HeighControl"):
if ((abs(self.targetZ - self.heigh) < self.minAltit)
and (currentState == "FollowRoad")):
self.sub1 = "FollowRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowRoad')
elif ((abs(self.targetZ - self.heigh) < self.minAltit)
and (currentState == "MonitorArea")):
self.sub1 = "MonitorArea"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('MonitorArea')
# Actuation if
if (self.sub1 == "TakeOff"):
#Taking Off
self.ExtraPrx.takeoff()
takenOff = True
elif (self.sub1 == "FollowRoad"):
lastState = "FollowRoad"
#Get and prepare image
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
hsvImg = cv2.cvtColor(softenedImg, cv2.COLOR_BGR2HSV)
#Get threshold image
minValues = numpy.array([self.hmin, self.vmin, self.smin])
maxValues = numpy.array([self.hmax, self.vmax, self.smax])
thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)
#Get contours
self.contours, hierarchy = cv2.findContours(
thresholdImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
#Update Heigh
self.heigh = self.Pose3DPrx.getPose3DData().z
elif (self.sub1 == "TurnAround"):
self.setVelocity(0, 0, 0, 1)
pose = self.Pose3DPrx.getPose3DData()
print "q0", pose.q0, "q1", pose.q1, "q2", pose.q2, "q3", pose.q3
elif (self.sub1 == "END"):
print "SHUT DOWN"
self.shutDown()
elif (self.sub1 == "HeighControl"):
#Controling heigh
self.heigh = self.Pose3DPrx.getPose3DData().z
zVel = self.zPid.process(self.targetZ - self.heigh)
self.setVelocity(0, 0, zVel, 0)
print "Heigh:", self.heigh
print "Vel:", zVel
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run3):
totala = time.time() * 1000000
if (self.sub1 == "FollowRoad"):
if ((self.sub3 == "FindRoad_ghost")
or (self.sub3 == "FollowingRoad_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub3 == "FindRoad"):
if (self.contours):
self.sub3 = "FollowingRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowingRoad')
elif (self.sub3 == "FollowingRoad"):
if (not self.contours):
self.sub3 = "FindRoad"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FindRoad')
# Actuation if
if (self.sub3 == "FindRoad"):
self.setVelocity(0, 0, 0.75, 0)
print "Road lost"
elif (self.sub3 == "FollowingRoad"):
contour = max(self.contours, key=cv2.contourArea)
try:
(x, y), radius = cv2.minEnclosingCircle(contour)
center = (int(x), int(y))
ellipse = cv2.fitEllipse(contour)
inclination = ellipse[2]
if inclination < 90:
yAngle = -inclination
else:
yAngle = 180 - inclination
avel = self.aPid.process(yAngle)
xError = self.targetX - center[0]
xvel = self.xPid.process(xError)
speed = max(
(self.targetX - pow(1.09, abs(xError))) / 125., 0)
self.setVelocity(xvel, speed, 0, avel)
except:
print "EXCEPTION FOUND"
self.contours = []
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
print "Xpos:", self.xPos, "Ypos:", self.yPos
else:
if (self.sub3 == "FindRoad"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif (self.sub3 == "FollowingRoad"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata5(self):
self.run5 = True
cycle = 100
t_activated = False
t_fin = 0
firstPos = (10, -20)
secondPos = (5, -30)
thirdPos = (-10, -20)
self.minDist = 0.02
#Control PID
xDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)
yDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)
while (self.run5):
totala = time.time() * 1000000
if (self.sub1 == "MonitorArea"):
if ((self.sub5 == "ToFirstPos_ghost")
or (self.sub5 == "ToSecondPos_ghost")
or (self.sub5 == "ToThirdPos_ghost")
or (self.sub5 == "Return_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub5 == "ToFirstPos"):
if (self.droneInPosition(firstPos)):
self.sub5 = "ToSecondPos"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'ToSecondPos')
elif (self.sub5 == "ToSecondPos"):
if (self.droneInPosition(secondPos)):
self.sub5 = "ToThirdPos"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'ToThirdPos')
elif (self.sub5 == "ToThirdPos"):
if (self.droneInPosition(thirdPos)):
self.sub5 = "Return"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Return')
elif (self.sub5 == "Return"):
if (self.droneInPosition(self.initPos)):
self.sub5 = "ToFirstPos"
self.monitorComplet = True
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'ToFirstPos')
# Actuation if
if (self.sub5 == "ToFirstPos"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = firstPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = firstPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "Xerror:", xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
elif (self.sub5 == "ToSecondPos"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = secondPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = secondPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "xError:", xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
elif (self.sub5 == "ToThirdPos"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = thirdPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = thirdPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "Xerror:", xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", yVel
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
elif (self.sub5 == "Return"):
self.xPos = self.Pose3DPrx.getPose3DData().x
self.yPos = self.Pose3DPrx.getPose3DData().y
xError = self.initPos[0] - self.xPos
xVel = xDistPid.process(xError)
yError = self.initPos[1] - self.yPos
yVel = yDistPid.process(yError)
print "Xerror:", xError, "Yerror:", yError
print "Xvel:", xVel, "Yvel:", -yVel
self.setVelocity(xVel, -yVel, 0, 0)
else:
if (self.sub5 == "ToFirstPos"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "ToSecondPos"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "ToThirdPos"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif (self.sub5 == "Return"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata6(self):
self.run6 = True
cycle = 100
t_activated = False
t_fin = 0
targetZDescend = 0.5
while (self.run6):
totala = time.time() * 1000000
if (self.sub1 == "Landing"):
if ((self.sub6 == "Descending_ghost")
or (self.sub6 == "Land_ghost")):
ghostStateIndex = self.StatesSub6.index(self.sub6)
self.sub6 = self.StatesSub6[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub6 == "Descending"):
if (abs(targetZDescend - self.heigh) < self.minAltit):
self.sub6 = "Land"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Land')
# Actuation if
if (self.sub6 == "Descending"):
#Controling heigh
self.heigh = self.Pose3DPrx.getPose3DData().z
zVel = self.zPid.process(targetZDescend - self.heigh)
self.setVelocity(0, 0, zVel, 0)
print "Heigh:", self.heigh
elif (self.sub6 == "Land"):
self.setVelocity(0, 0, 0, 0)
if not self.hasLanded:
self.ExtraPrx.land()
self.hasLanded = True
else:
if (self.sub6 == "Descending"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif (self.sub6 == "Land"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Extra
Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if (not Extra):
raise Exception('could not create proxy with Extra')
self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
if (not self.ExtraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'Extra connected'
# Contact to CMDVel
CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if (not CMDVel):
raise Exception('could not create proxy with CMDVel')
self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
if (not self.CMDVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'CMDVel connected'
# Contact to Pose3D
Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if (not Pose3D):
raise Exception('could not create proxy with Pose3D')
self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
if (not self.Pose3DPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'Pose3D connected'
# Contact to Camera
Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if (not Camera):
raise Exception('could not create proxy with Camera')
self.CameraPrx = CameraPrx.checkedCast(Camera)
if (not self.CameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'Camera connected'
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t5.join()
self.t6.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"GoStraight",
"TurnRight",
"MoveLeft",
"MoveRight",
]
self.sub1 = "GoStraight"
self.run1 = True
def get_min_distance(self):
laser_data = self.laser_sensor.getLaserData()
min_dist = 100000
for i in range(laser_data.numLaser):
if i < 5:
continue
avg_dist = 0
for j in range(5):
avg_dist += laser_data.distanceData[i - j]
avg_dist = avg_dist / 5.0
if avg_dist < min_dist:
min_dist = avg_dist
print('min_dist:' + str(min_dist))
return min_dist
def get_left_distance(self):
laser_data = self.laser_sensor.getLaserData()
avg_dist = 0
for i in range(10):
avg_dist += laser_data.distanceData[laser_data.numLaser - i]
avg_dist = avg_dist / 10.0
print('avg_dist:' + str(avg_dist))
return avg_dist
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 107, 124, 1, 'GoStraight')
guiSubautomata1.newGuiNode(2, 0, 300, 128, 0, 'TurnRight')
guiSubautomata1.newGuiNode(3, 0, 457, 131, 0, 'MoveLeft')
guiSubautomata1.newGuiNode(4, 0, 464, 290, 0, 'MoveRight')
guiSubautomata1.newGuiTransition((107, 124), (300, 128), (190, 59), 1,
1, 2)
guiSubautomata1.newGuiTransition((300, 128), (457, 131), (369, 59), 2,
2, 3)
guiSubautomata1.newGuiTransition((457, 131), (464, 290), (553, 205), 4,
3, 4)
guiSubautomata1.newGuiTransition((464, 290), (457, 131), (390, 209), 5,
4, 3)
guiSubautomataList.append(guiSubautomata1)
return guiSubautomataList
def shutDown(self):
self.run1 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run1):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "GoStraight"):
if (self.get_min_distance() < 700):
self.sub1 = "TurnRight"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('TurnRight')
elif (self.sub1 == "TurnRight"):
if (self.get_left_distance() < 750):
self.sub1 = "MoveLeft"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('MoveLeft')
elif (self.sub1 == "MoveLeft"):
if (self.get_left_distance() < 700):
self.sub1 = "MoveRight"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('MoveRight')
elif (self.sub1 == "MoveRight"):
if (self.get_left_distance() > 700):
self.sub1 = "MoveLeft"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('MoveLeft')
# Actuation if
if (self.sub1 == "GoStraight"):
self.my_motors.setV(0.2)
self.my_motors.setW(0.0)
elif (self.sub1 == "TurnRight"):
self.my_motors.setV(0.0)
self.my_motors.setW(-0.2)
elif (self.sub1 == "MoveLeft"):
self.my_motors.setV(0.15)
self.my_motors.setW(0.15)
elif (self.sub1 == "MoveRight"):
self.my_motors.setV(0.15)
self.my_motors.setW(-0.15)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = EasyIce.initialize(sys.argv)
# Contact to laser_sensor
laser_sensor = self.ic.propertyToProxy('automata.laser_sensor.Proxy')
if (not laser_sensor):
raise Exception('could not create proxy with laser_sensor')
self.laser_sensor = LaserPrx.checkedCast(laser_sensor)
if (not self.laser_sensor):
raise Exception('invalid proxy automata.laser_sensor.Proxy')
print('laser_sensor connected')
# Contact to my_motors
my_motors = self.ic.propertyToProxy('automata.my_motors.Proxy')
if (not my_motors):
raise Exception('could not create proxy with my_motors')
self.my_motors = MotorsPrx.checkedCast(my_motors)
if (not self.my_motors):
raise Exception('invalid proxy automata.my_motors.Proxy')
print('my_motors connected')
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print('runtime gui enabled')
else:
self.displayGui = False
print('runtime gui disabled')
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"FollowGreenLookBlue",
"FollowBlueLookGreen",
"FollowGreenLookRed",
"FollowRed",
]
self.StatesSub3 = [
"WaitGreen",
"WaitGreen_ghost",
"FollowGreen",
"FollowGreen_ghost",
]
self.StatesSub4 = [
"WaitBlue",
"WaitBlue_ghost",
"FollowBlue",
"FollowBlue_ghost",
]
self.StatesSub5 = [
"WaitGreen2",
"WaitGreen2_ghost",
"FollowGreen2",
"FollowGreen2_ghost",
]
self.StatesSub6 = [
"WaitRed",
"WaitRed_ghost",
"FollowingRed",
"FollowingRed_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub3 = "WaitGreen_ghost"
self.run3 = True
self.sub4 = "WaitBlue_ghost"
self.run4 = True
self.sub5 = "WaitGreen2_ghost"
self.run5 = True
self.sub6 = "WaitRed_ghost"
self.run6 = True
def getImage(self):
img = self.CameraPrx.getImageData("RGB8")
height = img.description.height
width=img.description.width
if self.targetX == None and self.targetY == None:
self.targetX = width/2
self.targetY = height/2
print "targetX:", self.targetX, "targetY:,", self.targetY
imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
imgPixels.shape = height, width, 3
return imgPixels
def getContours(self, img, minValues, maxValues):
hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)
contours, hierarchy = cv2.findContours(thresholdImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
return contours, hierarchy
def getVelocities(self, contours):
contour = max(contours, key=cv2.contourArea)
(x, y), radius = cv2.minEnclosingCircle(contour)
xError = self.targetX - x
yError = self.targetY - y
xVel = self.xPID.process(xError)
yVel = self.yPID.process(yError)
return xVel, yVel
def setVelocity(self, vx, vy, vz, yaw):
cmd = jderobot.CMDVelData()
cmd.linearX = vy
cmd.linearY = vx
cmd.linearZ = vz
cmd.angularZ = yaw
cmd.angularX = cmd.angularY = 1.0
self.CMDVelPrx.setCMDVelData(cmd)
class PID:
def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
self.Ep = Epsilon
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.Imax = Imax
self.Imin = Imin
self.lastError = 0
self.cumulativeError = 0
def updateCumulativeError(self, error):
self.cumulativeError += error
if self.cumulativeError > self.Imax:
self.cumulativeError = self.Imax
elif self.cumulativeError < self.Imin:
self.cumulativeError = self.Imin
def process(self, error, derivative=None, integral=None):
if -self.Ep < error < self.Ep:
return 0
P = self.Kp * error
self.updateCumulativeError(error)
if integral != None:
I = self.Ki * integral
else:
I = self.Ki * self.cumulativeError
if derivative != None:
D = self.Kd * derivative
else:
D = self.Kd * (error - self.lastError)
self.lastError = error
speed = P + I + D
#if speed > 0.4:
# speed = 0.4
#if speed < -0.4:
# speed = -0.4
return speed
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t4 = threading.Thread(target=self.subautomata4)
self.t4.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
self.t6 = threading.Thread(target=self.subautomata6)
self.t6.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 117, 139, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 3, 303, 150, 0, 'FollowGreenLookBlue')
guiSubautomata1.newGuiNode(5, 4, 421, 244, 0, 'FollowBlueLookGreen')
guiSubautomata1.newGuiNode(6, 5, 325, 312, 0, 'FollowGreenLookRed')
guiSubautomata1.newGuiNode(7, 6, 142, 326, 0, 'FollowRed')
guiSubautomata1.newGuiTransition((303, 150), (421, 244), (421, 157), 4, 2, 5)
guiSubautomata1.newGuiTransition((421, 244), (325, 312), (436, 325), 5, 5, 6)
guiSubautomata1.newGuiTransition((325, 312), (142, 326), (232, 331), 6, 6, 7)
guiSubautomata1.newGuiTransition((117, 139), (303, 150), (194, 174), 15, 1, 2)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3,2, self.automataGui)
guiSubautomata3.newGuiNode(3, 0, 67, 147, 1, 'WaitGreen')
guiSubautomata3.newGuiNode(4, 0, 269, 152, 0, 'FollowGreen')
guiSubautomata3.newGuiTransition((67, 147), (269, 152), (173, 40), 2, 3, 4)
guiSubautomata3.newGuiTransition((269, 152), (67, 147), (170, 221), 3, 4, 3)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata4
guiSubautomata4 = GuiSubautomata(4,5, self.automataGui)
guiSubautomata4.newGuiNode(8, 0, 111, 129, 1, 'WaitBlue')
guiSubautomata4.newGuiNode(9, 0, 296, 141, 0, 'FollowBlue')
guiSubautomata4.newGuiTransition((111, 129), (296, 141), (200, 60), 7, 8, 9)
guiSubautomata4.newGuiTransition((296, 141), (111, 129), (191, 198), 8, 9, 8)
guiSubautomataList.append(guiSubautomata4)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5,6, self.automataGui)
guiSubautomata5.newGuiNode(10, 0, 79, 120, 1, 'WaitGreen2')
guiSubautomata5.newGuiNode(11, 0, 336, 152, 0, 'FollowGreen2')
guiSubautomata5.newGuiTransition((336, 152), (79, 120), (196, 220), 9, 11, 10)
guiSubautomata5.newGuiTransition((79, 120), (336, 152), (217, 56), 10, 10, 11)
guiSubautomataList.append(guiSubautomata5)
# Creating subAutomata6
guiSubautomata6 = GuiSubautomata(6,7, self.automataGui)
guiSubautomata6.newGuiNode(12, 0, 104, 151, 1, 'WaitRed')
guiSubautomata6.newGuiNode(13, 0, 341, 163, 0, 'FollowingRed')
guiSubautomata6.newGuiTransition((104, 151), (341, 163), (221, 56), 11, 12, 13)
guiSubautomata6.newGuiTransition((341, 163), (104, 151), (225, 219), 12, 13, 12)
guiSubautomataList.append(guiSubautomata6)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run4 = False
self.run5 = False
self.run6 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
#ControlValues
minError = 10
self.targetX = None
self.targetY = None
self.hIters = 0
#maxHIters = 35 #For gazebo
maxHIters = 20 #For real
refracIters = 0
refracTime = 35
#Contours
self.greenConts = []
self.blueConts = []
self.redConts = []
#Filter values for simulator. Order: [H, S, V]
minGValues = numpy.array([30, 129, 33])
maxGValues = numpy.array([70, 255 , 190])
minBValues = numpy.array([0, 140, 37])
maxBValues = numpy.array([16, 255, 200])
minRValues = numpy.array([100, 209, 82])
maxRValues = numpy.array([153, 255, 200])
#Filter values for real. Order: [H, S, V]
#minGValues = numpy.array([31,0,120])
#maxGValues = numpy.array([70, 46 , 255])
#minBValues = numpy.array([9,126,175])
#maxBValues = numpy.array([58,209,255])
#minRValues = numpy.array([93,195,154])
#maxRValues = numpy.array([155,255,255])
#PID control
self.xPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)
self.yPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)
while(self.run1):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "TakeOff"):
if(self.hIters >= maxHIters):
self.sub1 = "FollowGreenLookBlue"
self.setVelocity(0,0,0,0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowGreenLookBlue')
elif(self.sub1 == "FollowGreenLookBlue"):
if(self.blueConts):
self.sub1 = "FollowBlueLookGreen"
print "Reseting Green Conts"
self.greenConts = []
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowBlueLookGreen')
elif(self.sub1 == "FollowBlueLookGreen"):
if(self.greenConts):
self.sub1 = "FollowGreenLookRed"
self.blueConts = []
refracIters = 0
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowGreenLookRed')
elif(self.sub1 == "FollowGreenLookRed"):
if(self.redConts):
self.sub1 = "FollowRed"
self.greenConts = []
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowRed')
# Actuation if
if(self.sub1 == "TakeOff"):
if self.hIters == 0:
self.ExtraPrx.takeoff()
print "taking off"
else:
#self.setVelocity(0,0,0.5,0)
self.setVelocity(0,0,1,0)
self.hIters += 1
print "ITERS: ", self.hIters
elif(self.sub1 == "FollowGreenLookBlue"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.greenConts, hierarchy = self.getContours(softenedImg, minGValues, maxGValues)
self.blueConts, hierarchy = self.getContours(softenedImg, minBValues, maxBValues)
elif(self.sub1 == "FollowBlueLookGreen"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.blueConts, hierarchy = self.getContours(softenedImg, minBValues, maxBValues)
if refracIters >= refracTime:
self.greenConts, hierarchy = self.getContours(softenedImg, minGValues, maxGValues)
print "inside refrac time"
else:
print "refrac Iters:", refracIters
refracIters += 1
elif(self.sub1 == "FollowGreenLookRed"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.greenConts, hierarchy = self.getContours(softenedImg, minGValues, maxGValues)
self.redConts, hierarchy = self.getContours(softenedImg, minRValues, maxRValues)
print "RED:", self.redConts
elif(self.sub1 == "FollowRed"):
inImg = self.getImage()
softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
self.redConts, hierarchy = self.getContours(softenedImg, minRValues, maxRValues)
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run3):
totala = time.time() * 1000000
if(self.sub1 == "FollowGreenLookBlue"):
if ((self.sub3 == "WaitGreen_ghost") or (self.sub3 == "FollowGreen_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub3 == "WaitGreen"):
if(self.greenConts):
self.sub3 = "FollowGreen"
print "Green Finded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowGreen')
elif(self.sub3 == "FollowGreen"):
if(not self.greenConts):
self.sub3 = "WaitGreen"
print "Green Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('WaitGreen')
# Actuation if
if(self.sub3 == "WaitGreen"):
self.setVelocity(0, 0, 0, 0)
elif(self.sub3 == "FollowGreen"):
xVel, yVel = self.getVelocities(self.greenConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if(self.sub3 == "WaitGreen"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "FollowGreen"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata4(self):
self.run4 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run4):
totala = time.time() * 1000000
if(self.sub1 == "FollowBlueLookGreen"):
if ((self.sub4 == "WaitBlue_ghost") or (self.sub4 == "FollowBlue_ghost")):
ghostStateIndex = self.StatesSub4.index(self.sub4)
self.sub4 = self.StatesSub4[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub4 == "WaitBlue"):
if(self.blueConts):
self.sub4 = "FollowBlue"
print "Blue Finded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowBlue')
elif(self.sub4 == "FollowBlue"):
if(not self.blueConts):
self.sub4 = "WaitBlue"
print "Blue Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('WaitBlue')
# Actuation if
if(self.sub4 == "WaitBlue"):
self.setVelocity(0, 0, 0, 0)
elif(self.sub4 == "FollowBlue"):
xVel, yVel = self.getVelocities(self.blueConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if(self.sub4 == "WaitBlue"):
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
elif(self.sub4 == "FollowBlue"):
ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
self.sub4 = self.StatesSub4[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata5(self):
self.run5 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run5):
totala = time.time() * 1000000
if(self.sub1 == "FollowGreenLookRed"):
if ((self.sub5 == "WaitGreen2_ghost") or (self.sub5 == "FollowGreen2_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub5 == "WaitGreen2"):
if(self.greenConts):
self.sub5 = "FollowGreen2"
print "Green2 finded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowGreen2')
elif(self.sub5 == "FollowGreen2"):
if(not self.greenConts):
self.sub5 = "WaitGreen2"
print "Green2 Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('WaitGreen2')
# Actuation if
if(self.sub5 == "WaitGreen2"):
self.setVelocity(0, 0, 0, 0)
elif(self.sub5 == "FollowGreen2"):
xVel, yVel = self.getVelocities(self.greenConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if(self.sub5 == "WaitGreen2"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
elif(self.sub5 == "FollowGreen2"):
ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
self.sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata6(self):
self.run6 = True
cycle = 100
t_activated = False
t_fin = 0
while(self.run6):
totala = time.time() * 1000000
if(self.sub1 == "FollowRed"):
if ((self.sub6 == "WaitRed_ghost") or (self.sub6 == "FollowingRed_ghost")):
ghostStateIndex = self.StatesSub6.index(self.sub6)
self.sub6 = self.StatesSub6[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub6 == "WaitRed"):
if(self.redConts):
self.sub6 = "FollowingRed"
print "Red FInded"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('FollowingRed')
elif(self.sub6 == "FollowingRed"):
if(not self.redConts):
self.sub6 = "WaitRed"
print "Red Lost"
self.setVelocity(0, 0, 0, 0)
if self.displayGui:
self.automataGui.notifySetNodeAsActive('WaitRed')
# Actuation if
if(self.sub6 == "WaitRed"):
self.setVelocity(0, 0, 0, 0)
elif(self.sub6 == "FollowingRed"):
xVel, yVel = self.getVelocities(self.redConts)
self.setVelocity(xVel, yVel, 0, 0)
else:
if(self.sub6 == "WaitRed"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
elif(self.sub6 == "FollowingRed"):
ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
self.sub6 = self.StatesSub6[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Extra
Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if(not Extra):
raise Exception('could not create proxy with Extra')
self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
if(not self.ExtraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'Extra connected'
# Contact to CMDVel
CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if(not CMDVel):
raise Exception('could not create proxy with CMDVel')
self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
if(not self.CMDVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'CMDVel connected'
# Contact to Camera
Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if(not Camera):
raise Exception('could not create proxy with Camera')
self.CameraPrx = CameraPrx.checkedCast(Camera)
if(not self.CameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'Camera connected'
# Contact to Pose3D
Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if(not Pose3D):
raise Exception('could not create proxy with Pose3D')
self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
if(not self.Pose3DPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'Pose3D connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t4.join()
self.t5.join()
self.t6.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.StatesSub1 = [
"TakeOff",
"Stabilizing1",
"GoFront",
"Stoping",
"Landing",
"Stabilizing2",
]
self.StatesSub3 = [
"A",
"A_ghost",
"B",
"B_ghost",
]
self.StatesSub5 = [
"C",
"C_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub3 = "A_ghost"
self.run3 = True
self.sub5 = "C_ghost"
self.run5 = True
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)
guiSubautomata1.newGuiNode(1, 3, 61, 101, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 0, 283, 75, 0, 'Stabilizing1')
guiSubautomata1.newGuiNode(3, 0, 497, 130, 0, 'GoFront')
guiSubautomata1.newGuiNode(4, 0, 489, 320, 0, 'Stoping')
guiSubautomata1.newGuiNode(5, 0, 250, 408, 0, 'Landing')
guiSubautomata1.newGuiNode(6, 0, 66, 283, 0, 'Stabilizing2')
guiSubautomata1.newGuiTransition((61, 101), (283, 75), (9, 212), 1, 1, 2)
guiSubautomata1.newGuiTransition((283, 75), (497, 130), (413, 79), 2, 2, 3)
guiSubautomata1.newGuiTransition((497, 130), (489, 320), (570, 239), 3, 3, 4)
guiSubautomata1.newGuiTransition((489, 320), (250, 408), (410, 411), 4, 4, 5)
guiSubautomata1.newGuiTransition((250, 408), (66, 283), (152, 338), 6, 5, 6)
guiSubautomata1.newGuiTransition((66, 283), (61, 101), (9, 212), 1, 6, 1)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3,1, self.automataGui)
guiSubautomata3.newGuiNode(7, 0, 175, 146, 1, 'A')
guiSubautomata3.newGuiNode(8, 5, 468, 201, 0, 'B')
guiSubautomata3.newGuiTransition((175, 146), (468, 201), (348, 79), 1, 7, 8)
guiSubautomata3.newGuiTransition((468, 201), (175, 146), (303, 215), 2, 8, 7)
guiSubautomataList.append(guiSubautomata3)
# Creating subAutomata5
guiSubautomata5 = GuiSubautomata(5,3, self.automataGui)
guiSubautomata5.newGuiNode(9, 0, 70, 146, 1, 'C')
guiSubautomata5.newGuiTransition((70, 146), (70, 146), (70, 186), 3, 9, 9)
guiSubautomataList.append(guiSubautomata5)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run3 = False
self.run5 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.automataGui.show()
app.exec_()
def subautomata1(self):
run = True
cycle = 100
t_activated = False
hasTakenOff = False
hasLanded = False
while(run):
totala = time.time() * 1000000
# Evaluation if
if(self.sub1 == "TakeOff"):
if(hasTakenOff):
self.sub1 = "Stabilizing1"
print "Going to Stabilizing"
self.automataGui.notifySetNodeAsActive('Stabilizing1')
elif(self.sub1 == "Stabilizing1"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 1.5):
self.sub1 = "GoFront"
t_activated = False
print "going to GoFront"
self.automataGui.notifySetNodeAsActive('GoFront')
elif(self.sub1 == "GoFront"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3):
self.sub1 = "Stoping"
t_activated = False
print "going to Stoping"
self.automataGui.notifySetNodeAsActive('Stoping')
elif(self.sub1 == "Stoping"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3.5):
self.sub1 = "Landing"
t_activated = False
print "going to Land"
self.automataGui.notifySetNodeAsActive('Landing')
elif(self.sub1 == "Landing"):
if(hasLanded):
self.sub1 = "Stabilizing2"
self.automataGui.notifySetNodeAsActive('Stabilizing2')
elif(self.sub1 == "Stabilizing2"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > 3):
self.sub1 = "TakeOff"
t_activated = False
print "Restarting"
self.automataGui.notifySetNodeAsActive('TakeOff')
# Actuation if
if(self.sub1 == "TakeOff"):
print "Taking Off"
self.lock.acquire()
self.extraPrx.takeoff()
self.lock.release()
hasTakenOff = True
elif(self.sub1 == "Stabilizing1"):
print "Stabilizing"
elif(self.sub1 == "GoFront"):
cmd = jderobot.CMDVelData()
cmd.linearX = 1
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdPrx.setCMDVelData(cmd)
self.lock.release()
elif(self.sub1 == "Stoping"):
cmd = jderobot.CMDVelData()
cmd.linearX = 0
cmd.linearY = 0
cmd.linearZ = 0
self.lock.acquire()
self.cmdPrx.setCMDVelData(cmd)
self.lock.release()
print "Stoping"
elif(self.sub1 == "Landing"):
print "Landing"
self.lock.acquire()
self.extraPrx.land()
self.lock.release()
hasLanded = True
elif(self.sub1 == "Stabilizing2"):
finished()
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata3(self):
run = True
cycle = 100
t_activated = False
t_fin = 0
t_A_max = 0.023
t_B_max = 0.055
while(run):
totala = time.time() * 1000000
if(self.sub1 == "TakeOff"):
if ((self.sub3 == "A_ghost") or (self.sub3 == "B_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub3 == "A"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_A_max):
self.sub3 = "B"
t_activated = False
self.automataGui.notifySetNodeAsActive('B')
t_A_max = 0.023
elif(self.sub3 == "B"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_B_max):
self.sub3 = "A"
t_activated = False
self.automataGui.notifySetNodeAsActive('A')
t_B_max = 0.055
# Actuation if
if(self.sub3 == "A"):
print "A"
elif(self.sub3 == "B"):
print "B"
else:
if(self.sub3 == "A"):
t_A_max = 0.023 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
sub3 = self.StatesSub3[ghostStateIndex]
elif(self.sub3 == "B"):
t_B_max = 0.055 - (t_fin - t_ini)
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def subautomata5(self):
run = True
cycle = 100
t_activated = False
t_C_max = 0.567
while(run):
totala = time.time() * 1000000
if(self.sub3 == "B"):
if ((self.sub5 == "C_ghost")):
ghostStateIndex = self.StatesSub5.index(self.sub5)
self.sub5 = self.StatesSub5[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if(self.sub5 == "C"):
if(not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if(secs > t_C_max):
self.sub5 = "C"
t_activated = False
self.automataGui.notifySetNodeAsActive('C')
t_C_max = 0.567
# Actuation if
if(self.sub5 == "C"):
print "C"
else:
if(self.sub5 == "C"):
t_C_max = 0.567 - (t_fin - t_ini)
ghostStateIndex = self.StateSub5.index(self.sub5) + 1
sub5 = self.StatesSub5[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000;
if(msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if(msecs < 33 ):
time.sleep(33 / 1000);
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to camera
camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if(not camera):
raise Exception('could not create proxy with camera')
self.cameraPrx = CameraPrx.checkedCast(camera)
if(not self.cameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'camera connected'
# Contact to pose3d
pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
if(not pose3d):
raise Exception('could not create proxy with pose3d')
self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
if(not self.pose3dPrx):
raise Exception('invalid proxy automata.Pose3D.Proxy')
print 'pose3d connected'
# Contact to cmd
cmd = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if(not cmd):
raise Exception('could not create proxy with cmd')
self.cmdPrx = CMDVelPrx.checkedCast(cmd)
if(not self.cmdPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'cmd connected'
# Contact to extra
extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if(not extra):
raise Exception('could not create proxy with extra')
self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
if(not self.extraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'extra connected'
# Contact to navdata
navdata = self.ic.propertyToProxy('automata.Navdata.Proxy')
if(not navdata):
raise Exception('could not create proxy with navdata')
self.navdataPrx = NavdataPrx.checkedCast(navdata)
if(not self.navdataPrx):
raise Exception('invalid proxy automata.Navdata.Proxy')
print 'navdata connected'
def destroyIc(self):
if(self.ic):
self.ic.destroy()
def start(self):
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
time.sleep(1)
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
self.t5 = threading.Thread(target=self.subautomata5)
self.t5.start()
def join(self):
self.guiThread.join()
self.t1.join()
self.t3.join()
self.t5.join()
class Automata():
def __init__(self):
self.lock = threading.Lock()
self.displayGui = False
self.StatesSub1 = [
"TakeOff",
"FollowTurtle",
"Land",
]
self.StatesSub2 = [
"FindTurtle",
"FindTurtle_ghost",
"FollowTurtle",
"FollowTurtle_ghost",
]
self.StatesSub3 = [
"LoseTurtle",
"LoseTurtle_ghost",
"Landing",
"Landing_ghost",
]
self.sub1 = "TakeOff"
self.run1 = True
self.sub2 = "FindTurtle_ghost"
self.run2 = True
self.sub3 = "LoseTurtle_ghost"
self.run3 = True
def setVelocity(self, vx, vy, vz, yaw):
cmd = jderobot.CMDVelData()
cmd.linearX = vy
cmd.linearY = vx
cmd.linearZ = vz
cmd.angularZ = yaw
cmd.angularX = cmd.angularY = 1.0
self.CMDVelPrx.setCMDVelData(cmd)
def getImage(self):
img = self.CameraPrx.getImageData("RGB8")
height = img.description.height
width = img.description.width
imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
imgPixels.shape = height, width, 3
return imgPixels
def getContours(self):
img = self.getImage()
img = cv2.GaussianBlur(img, (5, 5), 0)
hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
maxValues = numpy.array(self.hmax, self.vmax, self.smax)
minValues = numpy.array(self.hmin, self.vmin, self.smin)
thresoldImg = cv2.inRange(hsvImg, minValues, maxValues)
conts, hierarchy = cv2.findContours(thresoldImg, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
return conts, hierarchy
class PID:
def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
self.Ep = Epsilon
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.Imax = Imax
self.Imin = Imin
self.lastError = 0
self.cumulativeError = 0
def updateCumulativeError(self, error):
self.cumulativeError += error
if self.cumulativeError > self.Imax:
self.cumulativeError = self.Imax
if self.cumulativeError < self.Imin:
self.cumulativeError = self.Imin
def process(self, error):
if -self.Ep < error < self.Ep:
return 0
P = self.Kp * error
self.updateCumulativeError(error)
I = self.Ki * self.cumulativeError
D = self.Kd * (error - self.lastError)
self.lastError = error
vel = P + I + D
return vel
def startThreads(self):
self.t1 = threading.Thread(target=self.subautomata1)
self.t1.start()
self.t2 = threading.Thread(target=self.subautomata2)
self.t2.start()
self.t3 = threading.Thread(target=self.subautomata3)
self.t3.start()
def createAutomata(self):
guiSubautomataList = []
# Creating subAutomata1
guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)
guiSubautomata1.newGuiNode(1, 0, 111, 195, 1, 'TakeOff')
guiSubautomata1.newGuiNode(2, 2, 323, 206, 0, 'FollowTurtle')
guiSubautomata1.newGuiNode(3, 3, 531, 229, 0, 'Land')
guiSubautomata1.newGuiTransition((111, 195), (323, 206), (191, 244), 1,
1, 2)
guiSubautomata1.newGuiTransition((323, 206), (531, 229), (419, 267), 2,
2, 3)
guiSubautomataList.append(guiSubautomata1)
# Creating subAutomata2
guiSubautomata2 = GuiSubautomata(2, 2, self.automataGui)
guiSubautomata2.newGuiNode(4, 0, 118, 156, 1, 'FindTurtle')
guiSubautomata2.newGuiNode(5, 0, 410, 171, 0, 'FollowTurtle')
guiSubautomata2.newGuiTransition((118, 156), (410, 171), (276, 101), 3,
4, 5)
guiSubautomata2.newGuiTransition((410, 171), (118, 156), (258, 221), 4,
5, 4)
guiSubautomataList.append(guiSubautomata2)
# Creating subAutomata3
guiSubautomata3 = GuiSubautomata(3, 3, self.automataGui)
guiSubautomata3.newGuiNode(7, 0, 117, 188, 1, 'LoseTurtle')
guiSubautomata3.newGuiNode(8, 0, 378, 213, 0, 'Landing')
guiSubautomata3.newGuiTransition((378, 213), (117, 188), (271, 109), 6,
8, 7)
guiSubautomata3.newGuiTransition((117, 188), (378, 213), (250, 258), 7,
7, 8)
guiSubautomataList.append(guiSubautomata3)
return guiSubautomataList
def shutDown(self):
self.run1 = False
self.run2 = False
self.run3 = False
def runGui(self):
app = QtGui.QApplication(sys.argv)
self.automataGui = AutomataGui()
self.automataGui.setAutomata(self.createAutomata())
self.automataGui.loadAutomata()
self.startThreads()
self.automataGui.show()
app.exec_()
def subautomata1(self):
self.run1 = True
cycle = 100
t_activated = False
t_fin = 0
self.heighIters = 0
self.maxIters = 50
self.contours = []
#Filter Values
self.hmin = 50
self.hmax = 70
self.vmin = 20
self.vmax = 235
self.smin = 10
self.smax = 245
while (self.run1):
totala = time.time() * 1000000
# Evaluation if
if (self.sub1 == "TakeOff"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 2.5):
self.sub1 = "FollowTurtle"
t_activated = False
print "Iters:", self.heighIters
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowTurtle')
elif (self.sub1 == "FollowTurtle"):
if (not t_activated):
t_ini = time.time()
t_activated = True
else:
t_fin = time.time()
secs = t_fin - t_ini
if (secs > 90):
self.sub1 = "Land"
t_activated = False
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Land')
# Actuation if
if (self.sub1 == "TakeOff"):
self.ExtraPrx.takeoff()
#Get some heigh
self.setVelocity(0, 0, 1, 0)
self.heighIters += 1
elif (self.sub1 == "FollowTurtle"):
self.contours, hierarchy = self.getContours()
elif (self.sub1 == "Land"):
self.contours, hierarchy = self.getContours()
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata2(self):
self.run2 = True
cycle = 100
t_activated = False
t_fin = 0
targetX = 125
targetY = 125
minError = 8
xPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)
yPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)
while (self.run2):
totala = time.time() * 1000000
if (self.sub1 == "FollowTurtle"):
if ((self.sub2 == "FindTurtle_ghost")
or (self.sub2 == "FollowTurtle_ghost")):
ghostStateIndex = self.StatesSub2.index(self.sub2)
self.sub2 = self.StatesSub2[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub2 == "FindTurtle"):
if (self.contours):
self.sub2 = "FollowTurtle"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FollowTurtle')
elif (self.sub2 == "FollowTurtle"):
if (not self.contours):
self.sub2 = "FindTurtle"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'FindTurtle')
# Actuation if
if (self.sub2 == "FindTurtle"):
if self.heighIters <= self.maxIters:
self.setVelocity(0, 0, 1, 0)
self.heighIters += 1
print "iters:", self.heighIters
else:
print "max hight reached"
elif (self.sub2 == "FollowTurtle"):
#We assume there is only one green target on the screen
cnt = self.contours[0]
#Locate the minimal circle enclosing the contour
(x, y), radius = cv2.minEnclosingCircle(cnt)
center = (int(x), int(y))
radius = int(radius)
xError = targetX - center[0]
yError = targetY - center[1]
xVel = xPid.process(xError)
yVel = yPidprocess(yError)
#Control Heigh#
self.setVelocity(xVel, yVel, 0, 0)
print "xError:", xError, "yError:", yError
print "xVel:", xVel, "yVel:", yVel
else:
if (self.sub2 == "FindTurtle"):
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
elif (self.sub2 == "FollowTurtle"):
ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
self.sub2 = self.StatesSub2[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def subautomata3(self):
self.run3 = True
cycle = 100
t_activated = False
t_fin = 0
while (self.run3):
totala = time.time() * 1000000
if (self.sub1 == "Land"):
if ((self.sub3 == "LoseTurtle_ghost")
or (self.sub3 == "Landing_ghost")):
ghostStateIndex = self.StatesSub3.index(self.sub3)
self.sub3 = self.StatesSub3[ghostStateIndex - 1]
t_ini = time.time()
# Evaluation if
if (self.sub3 == "LoseTurtle"):
if (not self.contours):
self.sub3 = "Landing"
if self.displayGui:
self.automataGui.notifySetNodeAsActive('Landing')
elif (self.sub3 == "Landing"):
if (self.contours):
self.sub3 = "LoseTurtle"
if self.displayGui:
self.automataGui.notifySetNodeAsActive(
'LoseTurtle')
# Actuation if
if (self.sub3 == "LoseTurtle"):
self.setVelocity(1, 0, 0, 0)
elif (self.sub3 == "Landing"):
self.ExtraPrx.land()
else:
if (self.sub3 == "LoseTurtle"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
elif (self.sub3 == "Landing"):
ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
self.sub3 = self.StatesSub3[ghostStateIndex]
totalb = time.time() * 1000000
msecs = (totalb - totala) / 1000
if (msecs < 0 or msecs > cycle):
msecs = cycle
else:
msecs = cycle - msecs
time.sleep(msecs / 1000)
if (msecs < 33):
time.sleep(33 / 1000)
def connectToProxys(self):
self.ic = Ice.initialize(sys.argv)
# Contact to Extra
Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
if (not Extra):
raise Exception('could not create proxy with Extra')
self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
if (not self.ExtraPrx):
raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
print 'Extra connected'
# Contact to CMDVel
CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
if (not CMDVel):
raise Exception('could not create proxy with CMDVel')
self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
if (not self.CMDVelPrx):
raise Exception('invalid proxy automata.CMDVel.Proxy')
print 'CMDVel connected'
# Contact to Camera
Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
if (not Camera):
raise Exception('could not create proxy with Camera')
self.CameraPrx = CameraPrx.checkedCast(Camera)
if (not self.CameraPrx):
raise Exception('invalid proxy automata.Camera.Proxy')
print 'Camera connected'
def destroyIc(self):
if (self.ic):
self.ic.destroy()
def start(self):
if self.displayGui:
self.guiThread = threading.Thread(target=self.runGui)
self.guiThread.start()
else:
self.startThreads()
def join(self):
if self.displayGui:
self.guiThread.join()
self.t1.join()
self.t2.join()
self.t3.join()
def readArgs(self):
for arg in sys.argv:
splitedArg = arg.split('=')
if splitedArg[0] == '--displaygui':
if splitedArg[1] == 'True' or splitedArg[1] == 'true':
self.displayGui = True
print 'runtime gui enabled'
else:
self.displayGui = False
print 'runtime gui disabled'
