moved speech detection into controller

also the thing works quite good (as in demo-good)

launch/fulltest.launch

@@ -10,8 +10,6 @@
         output="screen"/>
     <node name="controller" pkg="teleoperation" type="controller.py"
         output="screen"/>
-    <node name="speech_client" pkg="teleoperation" type="speech_client.py"
-        output="screen"/>
     <node name="imitator" pkg="teleoperation" type="imitator.py"
         output="screen"/>
     <node name="walker" pkg="teleoperation" type="walker.py"/>

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

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)

script/speech_client.py

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

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)