teleoperation/script/masterloop.py

#! /usr/bin/env python
import os
from argparse import ArgumentParser

import rospy
import actionlib

from teleoperation.srv import InformMasterloop, InformMasterloopResponse
from teleoperation.srv import Hands
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
from naoqi import ALProxy


# Constants
IMITATE = 'arms'
KILL = 'kill'
REVIVE = 'go'
STOP = 'stop'
OPEN = 'open'
CLOSE = 'close'

VOC_STATE = None
SPEECH_TRANSITIONS = None
VOC_HAND = {
    'closed': [OPEN],
    'opened': [CLOSE]
}
SPEECH_HAND = (OPEN, CLOSE)
AI = False  # autoimitate


# Globals
state = 'dead'  # Also walk, imitate, fallen, idle
hand_state = 'closed'
speech_in_progress = False

mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)


def init_voc_state_speech():
    global VOC_STATE, SPEECH_TRANSITIONS
    VOC_STATE = {
        'idle': [KILL] + ([IMITATE] if not AI else []),
        'imitate': [KILL] + ([STOP] if not AI else []),
        'dead': [REVIVE]
    }
    SPEECH_TRANSITIONS = {
        KILL: (('idle', 'imitate'), 'dead'),
        REVIVE: (('dead',), 'idle')
    }
    if not AI:
        SPEECH_TRANSITIONS.update({
            IMITATE: (('idle',), 'imitate'),
            STOP: (('imitate',), 'idle')
        })


def hands(what):
    try:
        _hands = rospy.ServiceProxy('hands', Hands)
        newstate =  _hands(what).newstate
        return newstate if newstate in ('opened', 'closed') else hand_state
    except rospy.service.ServiceException:
        return hand_state


def speech_done_cb(_, result):
    global speech_in_progress, state, hand_state
    _state_old = state
    rospy.loginfo('SPEECH: {}'.format(result))

    if not result:
        speech_in_progress = False
        return
    if result.word in SPEECH_TRANSITIONS:
        allowed, target = SPEECH_TRANSITIONS[result.word]
        if state in allowed:
            state = target
    elif result.word in SPEECH_HAND:
        hand_state = hands(result.word)
    if _state_old != state:
        rospy.loginfo('{} -> {}'.format(_state_old, state))
    speech_in_progress = False


def inform_masterloop_factory(who):
    def inform_masterloop(what):
        try:
            inform_masterloop = rospy.ServiceProxy('inform_masterloop',
                                                   InformMasterloop)
            perm = inform_masterloop(who, what).permission
        except rospy.service.ServiceException:
            rospy.signal_shutdown('Masterloop is dead')
            perm = False
        return perm
    return inform_masterloop


def handle_request(r):
    global state
    module = r.module
    message = r.message
    _state_old = state
    permission = False

    if module == 'walker':
        if message == 'move':
            if state in ('idle', 'walk') + (('imitate',) if AI else ()):
                state = 'walk'
                permission = True
        elif message == 'stop':
            if state == 'walk':
                state = 'idle'
                permission = True

    elif module == 'fall_detector':
        permission = True
        if message in ('falling', 'fallen'):
            state = 'fallen'
        elif message == 'recovered':
            state = 'idle'

    elif module == 'imitator':
        if message == 'imitate':
            if state in ('imitate',) + (('idle',) if AI else ()):
                permission = True
                state = 'imitate'

    rospy.logdebug(
        'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nstate IS NOW: %s.' % (
            module, message, permission, state
        )
    )
    if _state_old != state:
        rospy.loginfo('{} -> {}'.format(_state_old, state))
    return InformMasterloopResponse(permission)


if __name__ == '__main__':

    rospy.init_node('masterloop')
    ap = ArgumentParser()
    aig = ap.add_mutually_exclusive_group(required=False)
    aig.add_argument('--autoimitate', dest='autoimitate',
                     action='store_true')
    aig.add_argument('--no-autoimitate', dest='autoimitate',
                     action='store_false')
    ap.set_defaults(autoimitate=False)
    args, _ = ap.parse_known_args()

    AI = args.autoimitate
    init_voc_state_speech()

    speech = actionlib.SimpleActionClient('speech_server',
                                          RequestSpeechAction)
    speech.wait_for_server()
    rospy.loginfo('SPEECH: SERVER THERE')

    mp.wakeUp()
    mp.moveInit()
    am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
    am.setExpressiveListeningEnabled(False)

    ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)

    def _shutdown():
        if speech_in_progress:
            speech.cancel_goal()
        mp.rest()

    rospy.on_shutdown(_shutdown)

    while not rospy.is_shutdown():
        if state in ('idle', 'imitate', 'dead'):
            if not speech_in_progress:
                speech_in_progress = True
                speech.send_goal(RequestSpeechGoal(
                    VOC_STATE[state] +
                    (VOC_HAND[hand_state] if state != 'dead' else [])
                ), speech_done_cb)
        else:
            if speech_in_progress:
                speech.cancel_goal()
                speech_in_progress = False

        rospy.Rate(10).sleep()