From 3217b7f841c7f7cae7048cb1b635a36dcb5e373f Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Thu, 31 Jan 2019 17:44:15 +0100 Subject: [PATCH] did some work --- action/RequestSpeech.action | 3 +- config/default.json | 10 ++ config/default.yaml | 10 ++ launch/fulltest.launch | 8 +- script/calibrator.py | 163 ++++++++++++++++++++++++- script/controller.py | 6 +- script/imitator.py | 18 +-- script/speech_client.py | 56 +++++++++ script/{speech.py => speech_server.py} | 53 +++++--- script/walker.py | 40 ++++-- 10 files changed, 323 insertions(+), 44 deletions(-) create mode 100644 config/default.json create mode 100644 config/default.yaml create mode 100755 script/speech_client.py rename script/{speech.py => speech_server.py} (59%) diff --git a/action/RequestSpeech.action b/action/RequestSpeech.action index 279b6a8..2286ab3 100644 --- a/action/RequestSpeech.action +++ b/action/RequestSpeech.action @@ -1,5 +1,4 @@ string[] vocabulary ---- -string result +string word ---- -bool success diff --git a/config/default.json b/config/default.json new file mode 100644 index 0000000..6bad3cd --- /dev/null +++ b/config/default.json @@ -0,0 +1,10 @@ +{ + "cr": [-1.95, 0.00, 0.00], + "fw": [-1.70, -1.40], + "bk": [-2.20, -2.40], + "lt": [ 0.35, 0.53], + "rt": [-0.35, -0.53], + "lr": [ 0.45, 0.85], + "rr": [-0.45, -0.85], + "arm": 70 +} diff --git a/config/default.yaml b/config/default.yaml new file mode 100644 index 0000000..f98a100 --- /dev/null +++ b/config/default.yaml @@ -0,0 +1,10 @@ +{ + "rt": -0.7155492901802063, + "rr": -0.6855806884976677, + "fw": -1.3561756610870361, + "bk": -2.5843331813812256, + "lt": 0.5666157603263855, + "lr": 0.6060229593671598, + "cr": [-1.9731173515319824, -0.04246790334582329, -0.050492866697747864], + "arm": 0.66392470071039278 +} diff --git a/launch/fulltest.launch b/launch/fulltest.launch index 67d40df..887ce79 100644 --- a/launch/fulltest.launch +++ b/launch/fulltest.launch @@ -3,18 +3,18 @@ - - + + - diff --git a/script/calibrator.py b/script/calibrator.py index 98fe0c6..146e57c 100755 --- a/script/calibrator.py +++ b/script/calibrator.py @@ -1,11 +1,172 @@ #! /usr/bin/env python +import os +import json +from sys import argv + import rospy +import tf +import actionlib +import numpy as np +from naoqi import ALProxy from controller import inform_controller_factory +from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal _inform_controller = inform_controller_factory('calibrator') +def calibration(): + tts.say('Stand in front of camera') + tts.say('Far enough to see your arms') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + trans, q = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + rot = tf.transformations.euler_from_quaternion(q) + center = trans, rot + + except Exception: + pass + + tts.say('Now stretch your arms') + tts.say('Make sure the markers are detected') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + larm, _ = ll.lookupTransform('Aruco_0_frame', + 'Aruco_1_frame', + rospy.Time(0)) + rarm, _ = ll.lookupTransform('Aruco_0_frame', + 'Aruco_2_frame', + rospy.Time(0)) + except Exception: + pass + + tts.say('Now rotate to your right') + tts.say('So that marker is still detected') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + _, q = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + RR = tf.transformations.euler_from_quaternion(q) + except Exception: + pass + + tts.say('Now rotate to your left') + tts.say('So that marker is still detected') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + _, q = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + LR = tf.transformations.euler_from_quaternion(q) + except Exception: + pass + + + tts.say('Now rotate to initial position') + tts.say('Take a big step forward') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + FW, _ = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + except Exception: + pass + + tts.say('Now return to the initial position') + tts.say('Then take a big step backward') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + BK, _ = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + except Exception: + pass + + tts.say('Now return to the initial position') + tts.say('Then take a big step to the right') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + RT, _ = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + except Exception: + pass + + tts.say('Now return to the initial position') + tts.say('Then take a big step to the left') + tts.say('Then say start') + client.send_goal(RequestSpeechGoal(['start'])) + client.wait_for_result() + + try: + LT, _ = ll.lookupTransform('odom', + 'Aruco_0_frame', + rospy.Time(0)) + except Exception: + pass + + calib = {} + + try: + tts.say('Well done') + calib['cr'] = [center[0][0], center[0][1], center[1][2]] + calib['arm'] = (np.linalg.norm(np.array(larm)) + + np.linalg.norm(np.array(rarm))) / 2 + calib['rr'] = RR[2] + calib['lr'] = LR[2] + calib['fw'] = FW[0] + calib['bk'] = BK[0] + calib['lt'] = LT[1] + calib['rt'] = RT[1] + here = os.path.dirname(os.path.realpath(__file__)) + with open('{}/../config/default.yaml'.format(here), 'w') as f: + json.dump(calib, f) + + except: + tts.say('Something has gone very wrong') + + if __name__ == '__main__': - rospy.init_node('teleoperation/calibrator') + if len(argv) < 2: + raise SystemExit + + rospy.init_node('calibrator', disable_signals=True) + + try: + tts = ALProxy('ALTextToSpeech', os.environ['NAO_IP'], 9559) + client = actionlib.SimpleActionClient('speech_server', + RequestSpeechAction) + client.wait_for_server() + + ll = tf.TransformListener() + + rospy.on_shutdown(lambda: client.cancel_goal()) + + calibration() + except KeyboardInterrupt: + rospy.signal_shutdown('Interrupted') diff --git a/script/controller.py b/script/controller.py index e4583d9..5042a3f 100755 --- a/script/controller.py +++ b/script/controller.py @@ -4,7 +4,7 @@ import rospy from teleoperation.srv import InformController, InformControllerResponse -STATE = 'idle' # Also walk, imitate and fallen +STATE = 'idle' # Also walk, imitate, fallen, recognizing def inform_controller_factory(who): @@ -49,7 +49,7 @@ def handle_request(r): if STATE == 'imitate': permission = True - elif module == 'speech_recognition': + elif module == 'speech': if message == 'recognize': if STATE in ('idle', 'imitate'): permission = True @@ -73,6 +73,6 @@ def handle_request(r): if __name__ == '__main__': - rospy.init_node('teleoperation/controller', log_level=rospy.INFO) + rospy.init_node('controller', log_level=rospy.INFO) ic = rospy.Service('inform_controller', InformController, handle_request) rospy.spin() diff --git a/script/imitator.py b/script/imitator.py index adf61c2..f31c37a 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -33,15 +33,15 @@ if __name__ == '__main__': # torso movement - try: - _, q = ll.lookupTransform('odom', - 'Aruco_0_frame', - rospy.Time(0)) - rot = tf.transformations.euler_from_quaternion(q) - mp.setAngles(['LHipYawPitch', 'RHipYawPitch'], - [-rot[1], -rot[1]], 0.3) - except Exception as e: - rospy.logwarn(e) + # try: + # _, q = ll.lookupTransform('odom', + # 'Aruco_0_frame', + # rospy.Time(0)) + # rot = tf.transformations.euler_from_quaternion(q) + # mp.setAngles(['LHipYawPitch', 'RHipYawPitch'], + # [-rot[1], -rot[1]], 0.3) + # except Exception as e: + # rospy.logwarn(e) # continue diff --git a/script/speech_client.py b/script/speech_client.py new file mode 100755 index 0000000..982c641 --- /dev/null +++ b/script/speech_client.py @@ -0,0 +1,56 @@ +#! /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' + +voc_state = { + 'idle': 'start', + 'imitate': 'stop' +} + +_inform_controller = inform_controller_factory('speech') + + +def done_cb(_, result): + global in_progress, state + rospy.loginfo(result) + if result.word == 'start' and _inform_controller('imitate'): + state = 'imitate' + elif result.word == 'stop' and _inform_controller('stop'): + 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' + continue + + if not in_progress: + in_progress = True + client.send_goal(RequestSpeechGoal([voc_state[state]]), + done_cb) + rospy.Rate(2).sleep() + diff --git a/script/speech.py b/script/speech_server.py similarity index 59% rename from script/speech.py rename to script/speech_server.py index 6866cb1..5d845a6 100755 --- a/script/speech.py +++ b/script/speech_server.py @@ -5,24 +5,29 @@ import rospy import actionlib from naoqi import ALProxy, ALModule, ALBroker +from teleoperation.msg import RequestSpeechAction, RequestSpeechResult + speech_broker = None almem = None -sas = actionlib.SimpleActionServer() +r = False def request_speech(goal): - recognized = '' - if not speech_detector.start_speech(goal): - sas.publish_feedback(False) + if not speech_detector.start_speech(goal.vocabulary): + sas.set_succeeded(RequestSpeechResult(word='')) return + while not sas.is_preempt_requested() and not speech_detector.have_word(): rospy.Rate(10).sleep() - speech_detector.stop_speech(goal) + if speech_detector.have_word(): recognized = speech_detector.get_recognized_and_erase() - sas.publish_feedback(True) - sas.set_succeeded(recognized) + else: + speech_detector.stop_speech() + recognized = '' + + sas.set_succeeded(RequestSpeechResult(word=recognized)) class SpeechDetectorModule(ALModule): @@ -31,14 +36,16 @@ class SpeechDetectorModule(ALModule): ALModule.__init__(self, name) self.recognized = None self.subid = 'teleoperation_speech' + self.voc = [] self.asr = ALProxy('ALSpeechRecognition') + self.tts = ALProxy('ALTextToSpeech') self.asr.setLanguage('English') - self.stop_speech() + self.running = False def start_speech(self, voc): if self.running: return False - # self.stop_speech() + self.voc = voc self.asr.setVocabulary(voc, False) self.asr.subscribe(self.subid) self.asr.pause(False) @@ -54,16 +61,27 @@ class SpeechDetectorModule(ALModule): return result def stop_speech(self): - self.asr.pause(True) + if not self.running: + return self.asr.unsubscribe(self.subid) + self.asr.pause(True) self.running = False def on_word_recognized(self, *_args): - rospy.loginfo(almem.getData('WordRecognized')) + word, conf = almem.getData('WordRecognized') + if conf > 0.4: + self.stop_speech() + self.tts.say(word) + self.recognized = word + else: + self.stop_speech() + self.tts.say('I didn\'t understand. Please repeat') + self.start_speech(self.voc) + if __name__ == '__main__': - rospy.init_node('speech') + rospy.init_node('speech_server') speech_broker = ALBroker('speech_broker', '0.0.0.0', 0, os.environ['NAO_IP'], 9559) speech_detector = SpeechDetectorModule('speech_detector') @@ -71,11 +89,16 @@ if __name__ == '__main__': almem.subscribeToEvent("WordRecognized", "speech_detector", "on_word_recognized") - - speech_detector.start_speech(['start', 'stop', 'pause']) + speech_detector.asr.pause(True) + sas = actionlib.SimpleActionServer('speech_server', RequestSpeechAction, + execute_cb=request_speech, + auto_start=False) + sas.start() while not rospy.is_shutdown(): rospy.Rate(4).sleep() - speech_detector.stop_speech() + if speech_detector.running: + speech_detector.stop_speech() + speech_broker.shutdown() diff --git a/script/walker.py b/script/walker.py index 60c6ab3..679fcb3 100755 --- a/script/walker.py +++ b/script/walker.py @@ -1,5 +1,6 @@ #! /usr/bin/env python import os +import json from time import sleep import rospy @@ -9,18 +10,36 @@ from naoqi import ALProxy from controller import inform_controller_factory - #min #max -FW = -1.70, -1.40 -BK = -2.20, -2.40 -LT = 0.35, 0.53 -RT = -0.35, -0.53 -LR = 0.45, 0.85 -RR = -0.45, -0.85 +FW = None +BK = None +LT = None +RT = None +LR = None +RR = None VMIN = 0.3 VMAX = 1.0 +def thirdway(a, b): + return a + (b - a) / 3 + + +def global_init(): + global FW, BK, LT, RT, LR, RR + here = os.path.dirname(os.path.realpath(__file__)) + with open('{}/../config/default.yaml'.format(here)) as f: + x = json.load(f) + + cx, cy, cz = x['cr'] + FW = thirdway(cx, x['fw']), x['fw'] + BK = thirdway(cx, x['bk']), x['bk'] + LT = thirdway(cy, x['lt']), x['lt'] + RT = thirdway(cy, x['rt']), x['rt'] + LR = thirdway(cz, x['lr']), x['lr'] + RR = thirdway(cz, x['rr']), x['rr'] + + _inform_controller = inform_controller_factory('walker') @@ -37,11 +56,12 @@ def _speed(pos, interval): if __name__ == '__main__': - rospy.init_node('teleoperation/walker') + rospy.init_node('walker') + global_init() rospy.wait_for_service('inform_controller') ll = tf.TransformListener() mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) - # mp.wakeUp() + mp.wakeUp() mp.moveInit() mp.setMoveArmsEnabled(False, False) @@ -84,7 +104,7 @@ if __name__ == '__main__': if not permission: mp.stopMove() else: - # mp.moveToward(*movement) # DON'T DELETE THIS ONE + mp.moveToward(*movement) # DON'T DELETE THIS ONE pass mp.rest()