renamed controller to masterloop
This commit is contained in:
Pavel Lutskov
@@ -1,169 +0,0 @@
|
||||
#! /usr/bin/env python
|
||||
from argparse import ArgumentParser
|
||||
|
||||
import rospy
|
||||
import actionlib
|
||||
|
||||
from teleoperation.srv import InformController, InformControllerResponse
|
||||
from teleoperation.srv import Hands
|
||||
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
|
||||
|
||||
|
||||
# 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
|
||||
|
||||
|
||||
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_controller_factory(who):
|
||||
def inform_controller(what):
|
||||
try:
|
||||
inform_controller = rospy.ServiceProxy('inform_controller',
|
||||
InformController)
|
||||
perm = inform_controller(who, what).permission
|
||||
except rospy.service.ServiceException:
|
||||
rospy.signal_shutdown('Controller is dead')
|
||||
perm = False
|
||||
return perm
|
||||
return inform_controller
|
||||
|
||||
|
||||
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 InformControllerResponse(permission)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
rospy.init_node('controller')
|
||||
ap = ArgumentParser()
|
||||
ap.add_argument('-i', '--autoimitate',
|
||||
help='Switch between moving and imitating automatically',
|
||||
action='store_true')
|
||||
args, _ = ap.parse_known_args()
|
||||
|
||||
AI = args.autoimitate
|
||||
init_voc_state_speech()
|
||||
|
||||
ic = rospy.Service('inform_controller', InformController, handle_request)
|
||||
|
||||
speech = actionlib.SimpleActionClient('speech_server',
|
||||
RequestSpeechAction)
|
||||
|
||||
speech.wait_for_server()
|
||||
rospy.loginfo('SPEECH: SERVER THERE')
|
||||
|
||||
rospy.on_shutdown(lambda: speech_in_progress and speech.cancel_goal())
|
||||
|
||||
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]
|
||||
), speech_done_cb)
|
||||
else:
|
||||
if speech_in_progress:
|
||||
speech.cancel_goal()
|
||||
speech_in_progress = False
|
||||
|
||||
rospy.Rate(10).sleep()
|
||||
Reference in New Issue
Block a user