did some work

action/RequestSpeech.action

@@ -1,5 +1,4 @@
 string[] vocabulary
 ----
-string result
+string word
 ----
-bool success

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
+}

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
+}

launch/fulltest.launch

@@ -3,18 +3,18 @@
 <!--Launch file for Test of teleoperating NAO project-->
 
 <launch>
-    <include file="$(find nao_apps)/launch/speech.launch"/>
-    <node name="speech" pkg="teleoperation" type="speech" output="screen"/>
     <node name="usb_cam" pkg="usb_cam" type="usb_cam_node"/>
     <node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
         output="screen"/>
+    <node name="speech_server" pkg="teleoperation" type="speech_server.py"
+        output="screen"/>
+    <node name="speech_client" pkg="teleoperation" type="speech_client.py"
+        output="screen"/>
     <node name="controller" pkg="teleoperation" type="controller.py"
         output="screen"/>
     <node name="imitator" pkg="teleoperation" type="imitator.py"
         output="screen"/>
-    <!--
     <node name="walker" pkg="teleoperation" type="walker.py"/>
     <node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
-    -->
 
 </launch>

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')

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()

script/imitator.py

@@ -33,15 +33,15 @@ if __name__ == '__main__':
 
         # torso movement
 
-        try:
+        # try:
-            _, q = ll.lookupTransform('odom',
+            # _, q = ll.lookupTransform('odom',
-                                      'Aruco_0_frame',
+                                      # 'Aruco_0_frame',
-                                      rospy.Time(0))
+                                      # rospy.Time(0))
-            rot = tf.transformations.euler_from_quaternion(q)
+            # rot = tf.transformations.euler_from_quaternion(q)
-            mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
+            # mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
-                         [-rot[1], -rot[1]], 0.3)
+                         # [-rot[1], -rot[1]], 0.3)
-        except Exception as e:
+        # except Exception as e:
-            rospy.logwarn(e)
+            # rospy.logwarn(e)
 
         # continue
 

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()
+

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.vocabulary):
-    if not speech_detector.start_speech(goal):
+        sas.set_succeeded(RequestSpeechResult(word=''))
-        sas.publish_feedback(False)
         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)
+    else:
-    sas.set_succeeded(recognized)
+        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.asr.pause(True)
-    speech_detector.start_speech(['start', 'stop', 'pause'])
+    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()

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 = None
-FW = -1.70, -1.40
+BK = None
-BK = -2.20, -2.40
+LT = None
-LT =  0.35,  0.53
+RT = None
-RT = -0.35, -0.53
+LR = None
-LR =  0.45,  0.85
+RR = None
-RR = -0.45, -0.85
 
 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()