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, 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 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
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()
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
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)
