From f118be8cf20248b8fe42f120ccc83c66e94367c3 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Sat, 2 Feb 2019 14:59:56 +0100 Subject: [PATCH] moved speech detection into controller also the thing works quite good (as in demo-good) --- launch/fulltest.launch | 2 -- script/controller.py | 77 +++++++++++++++++++++++++++++------------ script/imitator.py | 24 +++++++++++-- script/speech_client.py | 68 ------------------------------------ script/speech_server.py | 2 +- 5 files changed, 78 insertions(+), 95 deletions(-) delete mode 100755 script/speech_client.py diff --git a/launch/fulltest.launch b/launch/fulltest.launch index f3d91cd..26f8163 100644 --- a/launch/fulltest.launch +++ b/launch/fulltest.launch @@ -10,8 +10,6 @@ output="screen"/> - diff --git a/script/controller.py b/script/controller.py index b8c9e73..f827985 100755 --- a/script/controller.py +++ b/script/controller.py @@ -1,10 +1,44 @@ #! /usr/bin/env python import rospy +import actionlib from teleoperation.srv import InformController, InformControllerResponse +from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal -STATE = 'idle' # Also walk, imitate, fallen, recognizing +STATE = 'dead' # Also walk, imitate, fallen, idle +speech_in_progress = False + +IMITATE = 'arms' +KILL = 'kill' +REVIVE = 'go' +STOP = 'stop' + +voc_state = { + 'idle': [IMITATE, KILL], + 'imitate': [STOP, KILL], + 'dead': [REVIVE] +} + + +def speech_done_cb(_, result): + global speech_in_progress, STATE + _state_old = STATE + rospy.loginfo('SPEECH: {}'.format(result)) + if not result: + speech_in_progress = False + return + if result.word == IMITATE and STATE == 'idle': + STATE = 'imitate' + elif result.word == STOP and STATE == 'imitate': + STATE = 'idle' + elif result.word == KILL and STATE in ('idle', 'imitate'): + STATE = 'dead' + elif result.word == REVIVE and STATE == 'dead': + STATE = 'idle' + if _state_old != STATE: + rospy.loginfo('{} -> {}'.format(_state_old, STATE)) + speech_in_progress = False def inform_controller_factory(who): @@ -49,26 +83,6 @@ def handle_request(r): if STATE == 'imitate': permission = True - elif module == 'speech': - if message == 'recognize': - if STATE in ('idle', 'imitate', 'dead'): - permission = True - elif message == 'imitate': - if STATE == 'idle': - STATE = 'imitate' - permission = True - elif message == 'stop': - if STATE == 'imitate': - STATE = 'idle' - permission = True - elif message == 'kill': - STATE = 'dead' - permission = True - elif message == 'revive': - if STATE == 'dead': - STATE = 'idle' - permission = True - rospy.logdebug( 'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nSTATE IS NOW: %s.' % ( module, message, permission, STATE @@ -80,6 +94,25 @@ def handle_request(r): if __name__ == '__main__': + rospy.init_node('controller', log_level=rospy.INFO) ic = rospy.Service('inform_controller', InformController, handle_request) - rospy.spin() + + client = actionlib.SimpleActionClient('speech_server', + RequestSpeechAction) + client.wait_for_server() + rospy.loginfo('SPEECH: SERVER THERE') + + rospy.on_shutdown(lambda: speech_in_progress and client.cancel_goal()) + + while not rospy.is_shutdown(): + if not STATE in ('idle', 'imitate', 'dead'): + if speech_in_progress: + client.cancel_goal() + speech_in_progress = False + else: + if not speech_in_progress: + speech_in_progress = True + client.send_goal(RequestSpeechGoal(voc_state[STATE]), + speech_done_cb) + rospy.Rate(10).sleep() diff --git a/script/imitator.py b/script/imitator.py index f31c37a..59f5677 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -1,17 +1,27 @@ #! /usr/bin/env python import os +import json from time import sleep from math import atan, asin, radians, sqrt import rospy import tf +import numpy as np from naoqi import ALProxy from controller import inform_controller_factory _inform_controller = inform_controller_factory('imitator') + + _FRAME_TORSO = 0 +arm = None + + +def _elbow(arm_): + quot = min(1.0, arm_ / arm) + return radians(180 * (1 - quot)) if __name__ == '__main__': @@ -25,6 +35,12 @@ if __name__ == '__main__': mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'], [radians(-70), radians(70), -radians(40), radians(40)], 0.5) + here = os.path.dirname(os.path.realpath(__file__)) + with open('{}/../config/default.yaml'.format(here)) as f: + x = json.load(f) + + arm = x['arm'] + while not rospy.is_shutdown(): sleep(0.1) if not _inform_controller('imitate'): @@ -63,12 +79,16 @@ if __name__ == '__main__': roll -= sign * radians(25) pitch = atan(-trans[2] / abs(trans[0])) + eroll = -sign * _elbow(np.linalg.norm(np.array(trans))) + mp.setAngles([ '{}ShoulderRoll'.format(eff), - '{}ShoulderPitch'.format(eff) + '{}ShoulderPitch'.format(eff), + '{}ElbowRoll'.format(eff) ], [ roll, - pitch + pitch, + eroll ], 0.3) # targ = np.array(trans) diff --git a/script/speech_client.py b/script/speech_client.py deleted file mode 100755 index f4e9dba..0000000 --- a/script/speech_client.py +++ /dev/null @@ -1,68 +0,0 @@ -#! /usr/bin/env python -# import os -# from time import sleep - -import rospy -import actionlib -# from naoqi import ALProxy - -from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal -from controller import inform_controller_factory - - -in_progress = False -state = 'idle' - -IMITATE = 'repeat' -KILL = 'kill' -REVIVE = 'go' -STOP = 'stop' - -voc_state = { - 'idle': [IMITATE, KILL], - 'imitate': [STOP, KILL], - 'killed': [REVIVE] -} - -_inform_controller = inform_controller_factory('speech') - - -def done_cb(_, result): - global in_progress, state - rospy.loginfo('SPEECH CLIENT: {}'.format(result)) - if result is None: - in_progress = False - return - if result.word == IMITATE and _inform_controller('imitate'): - state = 'imitate' - elif result.word == STOP and _inform_controller('stop'): - state = 'idle' - elif result.word == KILL and _inform_controller('kill'): - state = 'killed' - elif result.word == REVIVE and _inform_controller('revive'): - state = 'idle' - in_progress = False - - -if __name__ == '__main__': - rospy.init_node('speech_client') - client = actionlib.SimpleActionClient('speech_server', - RequestSpeechAction) - client.wait_for_server() - rospy.loginfo('SPEECH CLIENT: SERVER THERE') - - rospy.on_shutdown(lambda: client.cancel_goal()) - - while not rospy.is_shutdown(): - if not _inform_controller('recognize'): - if in_progress: - client.cancel_goal() - in_progress = False - state = 'idle' - - else: - if not in_progress: - in_progress = True - client.send_goal(RequestSpeechGoal(voc_state[state]), - done_cb) - rospy.Rate(4).sleep() diff --git a/script/speech_server.py b/script/speech_server.py index 1cc7e23..3302fe1 100755 --- a/script/speech_server.py +++ b/script/speech_server.py @@ -87,7 +87,7 @@ class SpeechDetectorModule(ALModule): self.stop_speech() else: self.stop_speech(pause=True) - self.tts.say('I didn\'t understand. Please repeat') + self.tts.say('nope') self.start_speech(self.voc, resume=True)