예제 #1
0
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()
예제 #2
0
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()
예제 #3
0
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)