teleoperation/script/masterloop.py

#! /usr/bin/env python
"""The master state machine node."""

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():
    """Initialize the transitions."""
    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):
    """Handle the command to do something with the hands."""
    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):
    """Handle the recognized command."""
    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):
    """Create a function to inform the masterloop.

    Every node infroms the master, so it's nice to have a factory for
    these functions.

    """
    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):
    """Handle a node's request to seize/release control."""
    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 = 'dead'

    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)

    hands('close')

    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()  # Run at 10 Hz or something