class NaoController:
def __init__(self, classifier_path, preprocessor_path, response_mapping):
self.response_mapping = response_mapping
self.robot = Nao()
self.camera_controller = CameraController()
self.frame_preprocessor = pickle.load(open(preprocessor_path, 'rb'))
self.classifier = pickle.load(open(classifier_path, 'rb'))
self._flusher = Thread(target=self._flush, daemon=True)
self._flusher.start()
def _flush(self):
while True:
self.camera_controller.flush()
def __del__(self):
del self.camera_controller
def step(self):
return self.robot.step()
def on_frame(self):
frame = self.camera_controller.get_frame()
if self.robot.is_moving():
return
processed = self.frame_preprocessor.preprocess(frame)
label = self.classifier.predict(np.array(
[processed]))[0] if processed is not None else None
self.robot.play_motion(self.response_mapping[label])
def __init__(self, classifier_path, preprocessor_path, response_mapping):
self.response_mapping = response_mapping
self.robot = Nao()
self.camera_controller = CameraController()
self.frame_preprocessor = pickle.load(open(preprocessor_path, 'rb'))
self.classifier = pickle.load(open(classifier_path, 'rb'))
self._flusher = Thread(target=self._flush, daemon=True)
self._flusher.start()
def addNao(self, name, adresseIP, port, naoqi_ver):
#nao management
naoId = len(self.list_of_nao)
nao = Nao(adresseIP, name, naoId, naoqi_ver)
self.list_of_nao.append(nao)
self.selection1.append(False)
self.selection2.append(False)
self.selectionGlobal.append(False)
#gui management
self.layoutNao.addWidget(nao, 3, naoId, QtCore.Qt.AlignCenter)
checkBox1 = QtGui.QCheckBox("1")
checkBox1.setStyleSheet("background-color:darkCyan")
checkBox2 = QtGui.QCheckBox("2")
checkBox2.setStyleSheet("background-color:darkYellow")
checkBoxGlobal = QtGui.QCheckBox("Global")
checkBoxGlobal.setStyleSheet("background-color:darkRed")
checkBox1.clicked.connect(
lambda: self.addselect(1, checkBox1.isChecked(), naoId))
checkBox2.clicked.connect(
lambda: self.addselect(2, checkBox2.isChecked(), naoId))
checkBoxGlobal.clicked.connect(
lambda: self.addselect(0, checkBoxGlobal.isChecked(), naoId))
self.list_of_selectionCheckBox1.append(checkBox1)
self.list_of_selectionCheckBox2.append(checkBox2)
self.list_of_selectionCheckBoxGlobal.append(checkBoxGlobal)
self.layoutNao.addWidget(checkBox1, 0, naoId, QtCore.Qt.AlignCenter)
self.layoutNao.addWidget(checkBox2, 1, naoId, QtCore.Qt.AlignCenter)
self.layoutNao.addWidget(checkBoxGlobal, 2, naoId,
QtCore.Qt.AlignCenter)
def __init__(self, args):
"""class constructor."""
self.NAO_IP = "nao.local"
self.NAO_PORT = 9559
self.motion = ALProxy("ALMotion", "nao.local", self.NAO_PORT)
self.nao = Nao()
self.names = ["nathan", "michel", "sophie", "delphine"]
print "_dialog instance have been instanciated"
def __init__(self):
"""instatiate qrRetreiver."""
"""class constructor."""
self.NAO_IP = "nao.local"
self.NAO_PORT = 9559
self.motion = ALProxy("ALMotion", "nao.local", self.NAO_PORT)
self.memory = ALProxy("ALMemory", self.NAO_IP, self.NAO_PORT)
self.nao = Nao()
pass
def build(self):
self.label = Label(text="server started\n")
self.factory = EchoFactory(self)
reactor.listenTCP(8000, self.factory)
rospy.init_node('twisted_node')
rospy.Subscriber("to_twisted", String, self.transmit_msg)
if STUDY_SITE == 'MIT':
self.publishers['tega'] = Tega(self)
elif STUDY_SITE == 'MIT-JIBO':
self.publishers['jibo'] = Jibo(self)
elif STUDY_SITE == 'TAU':
self.publishers['nao'] = Nao(self)
return self.label
class qrRetreiver:
"""docstring for qrRetreiver."""
def __init__(self):
"""instatiate qrRetreiver."""
"""class constructor."""
self.NAO_IP = "nao.local"
self.NAO_PORT = 9559
self.motion = ALProxy("ALMotion", "nao.local", self.NAO_PORT)
self.memory = ALProxy("ALMemory", self.NAO_IP, self.NAO_PORT)
self.nao = Nao()
pass
"""
REGION methodes
"""
def makeQr(self, text):
"""make a qrCode, with specified txt, returns it."""
code = pyqrcode.create(str(text))
code.png("./pics/" + str(text) + "_qr.png", scale=8)
content = open("./pics/" + str(text) + "_qr.png", "r").read()
return content
def getAll(self):
"""return all qrcodes."""
# reg = "(.*_qr)\.png\b"
reg = "*_qr.png"
ret_arr = []
for file in os.listdir("./pics"):
if fnmatch.fnmatch(file, reg):
ret_arr.append(file)
return ret_arr
def getImage(self):
"""main method, wait until qr code is found."""
period = 1000
qrCodeProxy = ALProxy("ALBarcodeReader", self.NAO_IP, self.NAO_PORT)
qrCodeProxy.subscribe("Testh_qr", period, 0.0)
detected = False
i = 0
while detected is False:
time.sleep(0.5)
val = self.memory.getData("BarcodeReader/BarcodeDetected")
print val
if val is not None:
if len(val) >= 1:
detected = True
todo = val[0][0]
ac = todo.split(" ", 1)
if len(ac) > 1:
action = self.nao.getAction().get(str(ac[0]))
action(str(ac[1]))
self.memory.insertData("BarcodeReader/BarcodeDetected", "")
else:
action = self.nao.getAction().get(todo)
action()
self.memory.insertData("BarcodeReader/BarcodeDetected", "")
i += 1
if i is 30:
detected = True
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--ip', help='addresse ip du nao')
parser.add_argument('--port', help='port du nao', type=int)
args = parser.parse_args()
nao = Nao(args.ip, args.port)
# FSM
states = ['idle', 'end', 'leftward', 'rightward', 'rest',
'forward', 'backward','shooting','shouting',
'lateralShuffelLeftward','lateralShuffelRightward']
machine = Machine(model=nao, states=states, transitions=[],
initial='idle', ignore_invalid_triggers=True)
# Ajout des états par états de destination
# Destination idle
sourceIdle = ['idle', 'leftward', 'rightward', 'rest', 'forward',
'backward', 'rest', 'joyMove',
'lateralShuffelLeftward', 'lateralShuffelRightward']
machine.add_transition(trigger='sleep', sourceIdle, dest='idle',
before='doSleep')
# Destination end
sourceEnd = ['idle', 'end', 'leftward', 'rightward','rest', 'forward',
'backward', 'joyMove', 'lateralShuffelLeftward',
'lateralShuffelRightward']
machine.add_transition(trigger='quit', sourceEnd, dest='end',
before='doQuit')
# Destination 'leftward'
sourceLeftward = ['leftward', 'rest', 'rightward', 'forward', 'backward',
'lateralShuffelLeftward', 'lateralShuffelRightward', 'joyMove']
machine.add_transition(trigger='left', sourceLeftward, dest='leftward',
before='doLeft')
# Destination 'rightward'
sourceRightward = ['leftward', 'rightward', 'rest', 'forward', 'backward',
'lateralShuffelLeftward', 'lateralShuffelRightward', 'joyMove']
machine.add_transition(trigger='right', sourceRightward, dest='rightward',
before='doRight')
# Destination 'rest'
sourceRest=['idle', 'leftward', 'rightward', 'rest', 'forward', 'backward',
'shooting','shouting', 'lateralShuffelLeftward',
'lateralShuffelRightward', 'joyMove']
machine.add_transition(trigger='standby', sourceRest, dest='rest',
before='doStandby')
# Destination 'forward'
sourceForward = ['leftward', 'rightward', 'rest', 'forward', 'backward',
'lateralShuffelLeftward', 'lateralShuffelRightward', 'joyMove']
machine.add_transition(trigger='go', sourceForward, dest='forward',
before='doGo')
# Destination backward
sourceBackward = ['leftward', 'rightward', 'rest', 'forward', 'backward',
'lateralShuffelLeftward', 'lateralShuffelRightward', 'joyMove']
machine.add_transition(trigger='goback', sourceBackward, dest='backward',
before='doGoback')
# Destination 'shooting'
sourceShooting = ['leftward', 'rightward', 'rest', 'forward', 'backward',
'lateralShuffelLeftward', 'lateralShuffelRightward', 'joyMove']
machine.add_transition(trigger='shoot', sourceShooting, dest='shooting',
before='doShoot')
# Destination 'shouting'
sourceShouting = ['rest']
machine.add_transition(trigger='shout', sourceShouting, dest='shouting',
before='doShout')
# Destination 'lateralShuffelLeftward'
sourceLateralShuffelLeftward = ['leftward', 'rightward', 'rest',
'forward','backward',
'lateralShuffelLeftward',
'lateralShuffelRightward']
machine.add_transition(trigger='lateralShuffelLeft',
sourceLateralShuffelLeftward,
dest='lateralShuffelLeftward',
before='doLateralShuffelLeft')
# Destination 'lateralShuffelRightward'
sourceLateralShuffelRightward = ['leftward', 'rightward', 'rest',
'forward','backward',
'lateralShuffelLeftward',
'lateralShuffelRightward', 'joyMove']
machine.add_transition(trigger='lateralShuffelRight',
sourceLateralShuffelRightward ,
dest='lateralShuffelRightward',
before='doLateralShuffelRight')
# Destination 'joyMove'
sourceJoystick = ['backward', 'foreward', 'joyMove',
'lateralShuffelLeftward', 'lateralShuffelRightward',
'leftward', 'rest', 'rightward']
machine.add_transition('joystick', sourceJoystick, 'joyMove',
before='doJoyMove')
# IHM
xref, yref, _ = nao.get_pos()
scale = 50
window_width = 640
window_height = 480
pg.init()
window = pg.display.set_mode((window_width, window_height))
detect_obstacle = True
pg.joystick.init()
jTolerance = 0.2
for i in range(pg.joystick.get_count()):
pg.joystick.Joystick(i).init()
run = True
while run:
for event in pg.event.get():
if event.type == JOYAXISMOTION:
if event.axis == 0:
if abs(event.value) < jTolerance:
nao.set_y_speed(0)
else:
nao.set_y_speed(event.value)
nao.joystick()
if event.axis == 1:
if abs(event.value) < jTolerance:
nao.set_x_speed(0)
else:
nao.set_x_speed(-event.value)
nao.joystick()
if event.axis == 2:
if abs(event.value) < jTolerance:
nao.set_rotation_speed(0)
else:
nao.set_rotation_speed(-event.value)
nao.joystick()
if event.type == JOYBUTTONDOWN:
if event.button == 0:
nao.shoot()
if event.type == KEYDOWN:
if event.key == K_DOWN:
nao.goback()
elif event.key == K_UP:
nao.go()
elif event.key == K_LEFT:
nao.left()
elif event.key == K_RIGHT:
nao.right()
elif event.key == K_SPACE:
nao.shoot()
elif event.key == K_q:
nao.quit()
elif event.key == K_RETURN:
nao.standby()
elif event.key == K_s:
nao.sleep()
elif event.key == K_o:
detect_obstacle = not detect_obstacle
if nao.state == 'end':
run = False
if nao.state == 'forward' and detect_obstacle:
nao.avoid_obstacle()
x, y, theta = nao.get_pos()
x = int((x-xref)*scale)+window_width/2
y = -int((y-yref)*scale)+window_height/2
window.fill((0, 255, 0))
pg.draw.circle(window, (0, 0, 255), (x, y), 6)
pg.display.flip()
pg.time.delay(100)
pg.quit()
exit()
class _dialog(ALModule):
"""
class _dialog :wrapper arround naoqi events.
contains handlers for nao,callbacks called by server.py.
"""
def __init__(self, args):
"""class constructor."""
self.NAO_IP = "nao.local"
self.NAO_PORT = 9559
self.motion = ALProxy("ALMotion", "nao.local", self.NAO_PORT)
self.nao = Nao()
self.names = ["nathan", "michel", "sophie", "delphine"]
print "_dialog instance have been instanciated"
def __call__(self):
"""return instance, usefull if called from othet script."""
return self
def readInstuction(self):
"""reads, and returns jsonified isntructions."""
instru = json.loads(open('../dialog.json').read())
return instru
def stopDialog(self):
"""join the threads, will close when stop is heard."""
print "before join"
self.thread.join()
print "after join"
def updateJson(self, jsonFile):
"""write json file in dialog.json."""
f = open('../dialog.json', 'w')
print jsonFile
f.write(str(jsonFile))
def createsFaceEvent(self):
"""triger facecallback when face is detected."""
print "in face event"
self.memory = ALProxy("ALMemory", self.NAO_IP, self.NAO_PORT)
faceProxy = ALProxy("ALFaceDetection", self.NAO_IP, self.NAO_PORT)
period = 500
faceProxy.subscribe("Face_detect", period, 0.0)
detected = False
i = 0
while detected is False:
time.sleep(0.5)
val = self.memory.getData("FaceDetected")
if(val and isinstance(val, list) and len(val) >= 2):
self.faceCallback()
detected = True
else:
print "i can't see anyone right now"
i += 1
if (i is 30):
detected = True
self.nao.say("je n'ai vu personne , cliques sur le bouton réessayer")
def startReco(self):
"""spawn face event in a new Thread."""
print "will spawn thread"
self.thread = Thread(target=self.createsFaceEvent)
self.thread.start()
def getJson(self):
"""read and return dialog.json as a string."""
instru = open('../dialog.json').read()
return instru
def faceCallback(self):
"""function called when nao detects a face."""
print "face detected"
self.nao.say("j'ai vu quelqu'un")
self.vocabulary = ["stop"]
for line in self.readInstuction():
self.vocabulary.append(str(line))
print "instru :", line
# print "je suis pret"
self.nao.setVocabulary(self.vocabulary)
self.createVoiceEvent()
time.sleep(1)
def createVoiceEvent(self):
"""called to init voice event."""
voc_list = self.readInstuction()
WordRecognized = False
i = 0
end = True
while WordRecognized is False:
time.sleep(1)
val = self.memory.getData("WordRecognized")
anws = val[0]
# anws = raw_input("what didu say ?\n")
print "u said : ", anws
if anws == "stop":
self.nao.say("ok j'arette")
end = True
WordRecognized = True
elif (anws in self.names):
print "mmm ur called ", anws
self.nao.say("ha tu es" + anws)
end = False
self.memory.insertData("WordRecognized", " ")
elif anws != "auto":
if anws == "nao":
end = False
WordRecognized = True
self.nao.say("c'est moi !")
self.memory.insertData("WordRecognized", " ")
elif anws in self.vocabulary:
WordRecognized = True
end = False
# print voc_list[anws], anws
text = random.choice(voc_list[anws])
print "i'll say :", str(text['anwser'])
self.nao.say(str(text['anwser']))
ac = text['action'].split(" ", 1)
if len(ac) > 1:
action = self.nao.getAction().get(str(ac[0]))
if action is not None:
action(str(ac[1]))
elif len(ac) <= 0:
action = self.nao.getAction().get(str(text['action']))
if action is not None:
action()
self.memory.insertData("WordRecognized", " ")
else:
pass
else:
end = False
i += 1
if (i is 30):
WordRecognized = True
if end is False:
self.createVoiceEvent()
def main():
"""A simple main"""
parser = argparse.ArgumentParser()
# Ottengo da terminale ip e porta
parser.add_argument(
"--ip",
type=str,
default="192.168.11.3",
help="Robot IP address. On robot or Local Naoqi: use '192.168.11.3'.")
parser.add_argument("--port",
type=int,
default=9559,
help="Naoqi port number")
args = parser.parse_args()
# Instanzio Nao, la connessione avviene automaticamente
global nao
nao = Nao(args.ip, args.port)
nao.hello()
# Inizializzo il microfono
print Fore.GREEN + "### INIT SPEECH-TO-TEXT ###"
print Style.RESET_ALL
stt.init()
# Apro un file strips di riferimento e quello che poi utlizzer per il planner
# Inizio la routine: ascolto comando-> ottengo il piano -> eseguo il piano
while True:
print Fore.GREEN + "### SESSION ###"
print Style.RESET_ALL
source_strips = open("strips_nao_example.txt", "r")
my_strips = open("strips_nao_test.txt", "w")
# Inizializzo il file strips con Initial State e Goal State
for index, line in enumerate(source_strips.readlines()):
if index == 0:
init_state_line = "Initial state: At(A)"
posture = nao.get_posture()
if posture == "Standing":
# potrebbe essere anche in un altra posizione
posture_line = "Posture(high)"
else:
posture_line = "Posture(low)"
init_state_line = init_state_line + "," + posture_line
hand_full = nao.hand_full()
if hand_full:
hand_line = "Hand(full),Bring(ball)"
else:
hand_line = "Hand(empty)"
init_state_line = init_state_line + "," + hand_line
print init_state_line
my_strips.write(init_state_line + "\n")
elif index == 1:
print(Fore.GREEN + "### SPEECH_TO_TEXT ###")
print(Style.RESET_ALL)
error = True
while error:
error = False
try:
speech = stt.listen()
except Exception as e:
print "Something was wrong with speech recognizing.Retrying.."
error = True
#speech = "prendi la palla"
print Fore.GREEN + "### TEXT ANALYSIS ###"
print Style.RESET_ALL
goals = tt.strips_goals(speech)
goal_state_line = "Goal state: "
if goals != "":
goal_state_line += goals
print goal_state_line
my_strips.write(goal_state_line)
else:
my_strips.write(line)
my_strips.flush()
source_strips.flush()
my_strips.close()
source_strips.close()
# Ottengo il piano(lista di stringhe)
print Fore.GREEN + "### STRIPS_PLANNER ###"
print Style.RESET_ALL
plan = sp.main()
print Fore.GREEN + "### NAO CLASS ###"
print Style.RESET_ALL
# elaborare il piano ()
try:
if plan != None:
for command in plan:
caratteri = list(command)
count = 0
for x in caratteri:
if x != "(":
count += 1
else:
break
# invoca un comando alla volta
nao.switcher(command[:count])
else:
pass
except KeyboardInterrupt:
print
print "Interrupted by user"
print "Stopping..."
nao.disconnect()
finally:
print Fore.GREEN + "### END SESSION ###"
print "\n"
print Style.RESET_ALL
@crossdomain(origin='*')
def get_latest_pictures():
"""Return the picture with provided name (0.png ...)."""
print 'here'
pic = glob.glob('./pics/' + request.args.get("n") + '.png')
print pic[0]
return send_file(pic[0], mimetype='image/png')
@app.route('/take_pic')
@crossdomain(origin='*')
def take_pic():
"""remove and permute old pic, save the new one."""
os.remove("./pics/2.png")
os.rename("./pics/1.png", "./pics/2.png")
os.rename("./pics/0.png", "./pics/1.png")
photoCaptureProxy = ALProxy("ALPhotoCapture", "192.168.1.8", 9559)
photoCaptureProxy.setResolution(2)
photoCaptureProxy.setPictureFormat("png")
photoCaptureProxy.takePictures(1, "/home/nao/NaoAccueil/www/Modules/python/pics", "_0")
img = Image.open("./pics/_0.png")
resultat = img.rotate(90, expand=1)
resultat.save("./pics/0.png")
return send_file("./pics/0.png", mimetype='image/png')
print "streamer"
nao = Nao()
nao.say("je suis pret !")
app.run(host='0.0.0.0', debug=False, threaded=True)
from nao import Nao
nao = Nao()
while True:
input()
print(nao.getPos())
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--ip', help='addresse ip du nao')
parser.add_argument('--port', help='port du nao', type=int)
args = parser.parse_args()
nao = Nao(args.ip, args.port)
# FSM
states = ['idle', 'end', 'leftward', 'rightward', 'rest',
'forward', 'backward','shooting']
transitions = [#état de départ idle
{'trigger': 'sleep', 'source': 'idle', 'dest': 'idle',
'before': 'doSleep'},
{'trigger': 'quit', 'source': 'idle', 'dest': 'end',
'before': 'doQuit'},
{'trigger': 'standby', 'source': 'idle', 'dest': 'rest',
'before': 'doWakeUp'},
#état de départ end,on ne peut pas sortir de l'état end
{'trigger': 'quit', 'source': 'end', 'dest': 'end',
'before': 'doQuit'},
#état de départ leftward
{'trigger': 'left', 'source': 'leftward', 'dest': 'leftward',
'before': 'doLeft'},
{'trigger': 'right', 'source': 'leftward', 'dest': 'rightward',
'before': 'doRight'},
{'trigger': 'standby', 'source': 'leftward', 'dest': 'rest',
'before': 'doStandby'},
{'trigger': 'go', 'source': 'leftward', 'dest': 'forward',
'before': 'doGo'},
{'trigger': 'goback', 'source': 'leftward', 'dest': 'backward',
'before': 'doGoback'},
{'trigger': 'shoot', 'source': 'leftward', 'dest': 'shooting',
'before': 'doShoot'},
{'trigger': 'sleep', 'source': 'leftward', 'dest': 'idle',
'before': 'doSleep'},
{'trigger': 'quit', 'source': 'leftward', 'dest': 'end',
'before': 'doQuit'},
#état de départ rightward
{'trigger': 'right', 'source': 'rightward', 'dest': 'rightward',
'before': 'doRight'},
{'trigger': 'standby', 'source': 'rightward', 'dest': 'rest',
'before': 'doStandby'},
{'trigger': 'go', 'source': 'rightward', 'dest': 'forward',
'before': 'doGo'},
{'trigger': 'goback', 'source': 'rightward', 'dest': 'backward',
'before': 'doGoback'},
{'trigger': 'shoot', 'source': 'rightward', 'dest': 'shooting',
'before': 'doShoot'},
{'trigger': 'sleep', 'source': 'rightward', 'dest': 'idle',
'before': 'doSleep'},
{'trigger': 'quit', 'source': 'rightward', 'dest': 'end',
'before': 'doQuit'},
{'trigger': 'left', 'source': 'rightward', 'dest': 'leftward',
'before': 'doLeft'},
#état de départ rest
{'trigger': 'standby', 'source': 'rest', 'dest': 'rest',
'before': 'doStandby'},
{'trigger': 'go', 'source': 'rest', 'dest': 'forward',
'before': 'doGo'},
{'trigger': 'goback', 'source': 'rest', 'dest': 'backward',
'before': 'doGoback'},
{'trigger': 'shoot', 'source': 'rest', 'dest': 'shooting',
'before': 'doShoot'},
{'trigger': 'sleep', 'source': 'rest', 'dest': 'idle',
'before': 'doSleep'},
{'trigger': 'quit', 'source': 'rest', 'dest': 'end',
'before': 'doQuit'},
{'trigger': 'left', 'source': 'rest', 'dest': 'leftward',
'before': 'doLeft'},
{'trigger': 'right', 'source': 'rest', 'dest': 'rightward',
'before': 'doRight'},
#état de départ forward
{'trigger': 'go', 'source': 'forward', 'dest': 'forward',
'before': 'doGo'},
{'trigger': 'goback', 'source': 'forward', 'dest': 'backward',
'before': 'doGoback'},
{'trigger': 'shoot', 'source': 'forward', 'dest': 'shooting',
'before': 'doShoot'},
{'trigger': 'sleep', 'source': 'forward', 'dest': 'idle',
'before': 'doSleep'},
{'trigger': 'quit', 'source': 'forward', 'dest': 'end',
'before': 'doQuit'},
{'trigger': 'left', 'source': 'forward', 'dest': 'leftward',
'before': 'doLeft'},
{'trigger': 'right', 'source': 'forward', 'dest': 'rightward',
'before': 'doRight'},
{'trigger': 'standby', 'source': 'forward', 'dest': 'rest',
'before': 'doStandby'},
#état de départ backward
{'trigger': 'goback', 'source': 'backward', 'dest': 'backward',
'before': 'doGoback'},
{'trigger': 'shoot', 'source': 'backward', 'dest': 'shooting',
'before': 'doShoot'},
{'trigger': 'sleep', 'source': 'backward', 'dest': 'idle',
'before': 'doSleep'},
{'trigger': 'quit', 'source': 'backward', 'dest': 'end',
'before': 'doQuit'},
{'trigger': 'left', 'source': 'backward', 'dest': 'leftward',
'before': 'doLeft'},
{'trigger': 'right', 'source': 'backward', 'dest': 'rightward',
'before': 'doRight'},
{'trigger': 'standby', 'source': 'backward', 'dest': 'rest',
'before': 'doStandby'},
{'trigger': 'go', 'source': 'backward', 'dest': 'forward',
'before': 'doGo'},
#état de départ shooting, après le tire le nao se remet droit
#et laisse la main au joueur
{'trigger': 'standby', 'source': 'shooting', 'dest': 'rest',
'before': 'doStandby'}]
machine = Machine(model=nao, states=states, transitions=transitions,
initial='idle', ignore_invalid_triggers=True)
xref, yref, _ = nao.get_pos()
scale = 50
l = 640
h = 480
pg.init()
window = pg.display.set_mode((l, h))
detect_obstacle = True
run = True
while run:
for event in pg.event.get():
if event.type == KEYDOWN:
if event.key == K_DOWN:
nao.goback()
elif event.key == K_UP:
nao.go()
elif event.key == K_LEFT:
nao.left()
elif event.key == K_RIGHT:
nao.right()
elif event.key == K_SPACE:
nao.shoot()
elif event.key == K_q:
nao.quit()
elif event.key == K_RETURN:
nao.standby()
elif event.key == K_s:
nao.sleep()
elif event.key == K_o:
detect_obstacle = !detect_obstacle
if nao.state == 'end':
run = False
if nao.state == 'forward' and detect_obstacle:
nao.avoid_obstacle()
x, y, theta = nao.get_pos()
x = int((x-xref)*scale)+l/2
y = -int((y-yref)*scale)+h/2
window.fill((0,255,0))
pg.draw.circle(window, (0,0,255), (x,y), 6)
pg.display.flip()
pg.time.delay(100)
pg.quit()
exit()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--ip', help='addresse ip du nao')
parser.add_argument('--port', help='port du nao', type=int)
args = parser.parse_args()
nao = Nao(args.ip, args.port)
# FSM
states = ['idle', 'end', 'turning_left', 'turning_right', 'rest',
'walking', 'walking_back']
transitions = [
{'trigger': 'go', 'source': 'idle', 'dest': 'walking',
'before': 'walk'},
{'trigger': 'wait', 'source': 'walking', 'dest': 'idle',
'before': 'stop_move'},
{'trigger': 'turn_left', 'source': 'idle', 'dest': 'turning_left',
'before': 'rotate_left'},
{'trigger': 'wait', 'source': 'turning_left', 'dest': 'idle',
'before': 'stop_move'},
{'trigger': 'turn_right', 'source': 'idle',
'dest': 'turning_right', 'before': 'rotate_right'},
{'trigger': 'wait', 'source': 'turning_right', 'dest': 'idle',
'before': 'stop_move'},
{'trigger': 'back', 'source': 'idle', 'dest': 'walking_back',
'before': 'walk_back'},
{'trigger': 'wait', 'source': 'walking_back', 'dest': 'idle',
'before': 'stop_move'},
{'trigger': 'stop', 'source': 'rest', 'dest': 'end'},
{'trigger': 'rest', 'source': 'idle', 'dest': 'rest',
'before': 'crouch'},
{'trigger': 'go', 'source': 'rest', 'dest': 'idle',
'before': 'wake_up'}
]
machine = Machine(model=nao, states=states, transitions=transitions,
initial='idle', ignore_invalid_triggers=True)
pg.init()
window = pg.display.set_mode((640, 480))
run = True
while run:
for event in pg.event.get():
if event.type == KEYDOWN:
if event.key == K_b:
nao.back()
elif event.key == K_c:
nao.rest()
elif event.key == K_g:
nao.go()
elif event.key == K_l:
nao.turn_left()
elif event.key == K_r:
nao.turn_right()
elif event.key == K_s:
nao.stop()
elif event.key == K_w:
nao.wait()
if nao.state == 'end':
run = False
if nao.state == 'walking':
nao.avoid_obstacle()
time.sleep(0.1)