did hands

script/controller.py

@@ -5,6 +5,7 @@ import rospy
 import actionlib
 
 from teleoperation.srv import InformController, InformControllerResponse
+from teleoperation.srv import Hands
 from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
 
 
@@ -22,10 +23,7 @@ VOC_HAND = {
     'closed': [OPEN],
     'opened': [CLOSE]
 }
-SPEECH_HAND = {
-    OPEN: (('closed',), 'opened'),
-    CLOSE: (('opened',), 'closed')
-}
+SPEECH_HAND = (OPEN, CLOSE)
 AI = False  # autoimitate
 
 
@@ -53,6 +51,15 @@ def init_voc_state_speech():
         })
 
 
+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
@@ -66,10 +73,7 @@ def speech_done_cb(_, result):
         if state in allowed:
             state = target
     elif result.word in SPEECH_HAND:
-        allowed, target = SPEECH_HAND[result.word]
-        if hand_state in allowed:
-            hand_state = target
-
+        hand_state = hands(result.word)
     if _state_old != state:
         rospy.loginfo('{} -> {}'.format(_state_old, state))
     speech_in_progress = False
@@ -142,23 +146,24 @@ if __name__ == '__main__':
 
     ic = rospy.Service('inform_controller', InformController, handle_request)
 
-    client = actionlib.SimpleActionClient('speech_server',
+    speech = actionlib.SimpleActionClient('speech_server',
                                           RequestSpeechAction)
-    client.wait_for_server()
+
+    speech.wait_for_server()
     rospy.loginfo('SPEECH: SERVER THERE')
 
-    rospy.on_shutdown(lambda: speech_in_progress and client.cancel_goal())
+    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
-                client.send_goal(RequestSpeechGoal(
+                speech.send_goal(RequestSpeechGoal(
                     VOC_STATE[state] + VOC_HAND[hand_state]
                 ), speech_done_cb)
         else:
             if speech_in_progress:
-                client.cancel_goal()
+                speech.cancel_goal()
                 speech_in_progress = False
 
         rospy.Rate(10).sleep()