From 3e990eb9ce317e490f2f59bd1695995832556de2 Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@tum.de>
Date: Sat, 2 Feb 2019 16:53:54 +0100
Subject: [PATCH] about to implement hand controls

---
 script/controller.py | 27 +++++++++++++++++++++------
 1 file changed, 21 insertions(+), 6 deletions(-)

diff --git a/script/controller.py b/script/controller.py
index 15a293b..4b43610 100755
--- a/script/controller.py
+++ b/script/controller.py
@@ -1,6 +1,5 @@
 #! /usr/bin/env python
 from argparse import ArgumentParser
-from sys import argv
 
 import rospy
 import actionlib
@@ -14,13 +13,25 @@ IMITATE = 'arms'
 KILL = 'kill'
 REVIVE = 'go'
 STOP = 'stop'
+OPEN = 'open'
+CLOSE = 'close'
+
 VOC_STATE = None
 SPEECH_TRANSITIONS = None
+VOC_HAND = {
+    'closed': [OPEN],
+    'opened': [CLOSE]
+}
+SPEECH_HAND = {
+    OPEN: (('closed',), 'opened'),
+    CLOSE: (('opened',), 'closed')
+}
 AI = False  # autoimitate
 
 
 # Globals
 state = 'dead'  # Also walk, imitate, fallen, idle
+hand_state = 'closed'
 speech_in_progress = False
 
 
@@ -43,7 +54,7 @@ def init_voc_state_speech():
 
 
 def speech_done_cb(_, result):
-    global speech_in_progress, state
+    global speech_in_progress, state, hand_state
     _state_old = state
     rospy.loginfo('SPEECH: {}'.format(result))
 
@@ -54,6 +65,10 @@ def speech_done_cb(_, result):
         allowed, target = SPEECH_TRANSITIONS[result.word]
         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
 
     if _state_old != state:
         rospy.loginfo('{} -> {}'.format(_state_old, state))
@@ -115,7 +130,7 @@ def handle_request(r):
 
 if __name__ == '__main__':
 
-    print(argv)
+    rospy.init_node('controller')
     ap = ArgumentParser()
     ap.add_argument('-i', '--autoimitate',
                     help='Switch between moving and imitating automatically',
@@ -125,7 +140,6 @@ if __name__ == '__main__':
     AI = args.autoimitate
     init_voc_state_speech()
 
-    rospy.init_node('controller', log_level=rospy.INFO)
     ic = rospy.Service('inform_controller', InformController, handle_request)
 
     client = actionlib.SimpleActionClient('speech_server',
@@ -139,8 +153,9 @@ if __name__ == '__main__':
         if state in ('idle', 'imitate', 'dead'):
             if not speech_in_progress:
                 speech_in_progress = True
-                client.send_goal(RequestSpeechGoal(VOC_STATE[state]),
-                                 speech_done_cb)
+                client.send_goal(RequestSpeechGoal(
+                    VOC_STATE[state] + VOC_HAND[hand_state]
+                ), speech_done_cb)
         else:
             if speech_in_progress:
                 client.cancel_goal()