can switch between autoimitation and voice mode

script/controller.py

@@ -1,4 +1,7 @@
 #! /usr/bin/env python
+from argparse import ArgumentParser
+from sys import argv
+
 import rospy
 import actionlib
 
@@ -6,38 +9,54 @@ from teleoperation.srv import InformController, InformControllerResponse
 from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
 
 
-STATE = 'dead'  # Also walk, imitate, fallen, idle
-speech_in_progress = False
-
+# Constants
 IMITATE = 'arms'
 KILL = 'kill'
 REVIVE = 'go'
 STOP = 'stop'
+VOC_STATE = None
+SPEECH_TRANSITIONS = None
+AI = False  # autoimitate
 
-voc_state = {
-    'idle': [IMITATE, KILL],
-    'imitate': [STOP, KILL],
+
+# Globals
+state = 'dead'  # Also walk, imitate, fallen, idle
+speech_in_progress = False
+
+
+def init_voc_state_speech():
+    global VOC_STATE, SPEECH_TRANSITIONS
+    VOC_STATE = {
+        'idle': [KILL] + ([IMITATE] if not AI else []),
+        'imitate': [KILL] + ([STOP] if not AI else []),
         'dead': [REVIVE]
-}
+    }
+    SPEECH_TRANSITIONS = {
+        KILL: (('idle', 'imitate'), 'dead'),
+        REVIVE: (('dead',), 'idle')
+    }
+    if not AI:
+        SPEECH_TRANSITIONS.update({
+            IMITATE: (('idle',), 'imitate'),
+            STOP: (('imitate',), 'idle')
+        })
 
 
 def speech_done_cb(_, result):
-    global speech_in_progress, STATE
-    _state_old = STATE
+    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))
+    if result.word in SPEECH_TRANSITIONS:
+        allowed, target = SPEECH_TRANSITIONS[result.word]
+        if state in allowed:
+            state = target
+
+    if _state_old != state:
+        rospy.loginfo('{} -> {}'.format(_state_old, state))
     speech_in_progress = False
 
 
@@ -55,46 +74,57 @@ def inform_controller_factory(who):
 
 
 def handle_request(r):
-    global STATE
+    global state
     module = r.module
     message = r.message
-    _state_old = STATE
+    _state_old = state
     permission = False
 
     if module == 'walker':
         if message == 'move':
-            if STATE in ('idle', 'walk'):
-                STATE = 'walk'
+            if state in ('idle', 'walk') + (('imitate',) if AI else ()):
+                state = 'walk'
                 permission = True
         elif message == 'stop':
-            if STATE == 'walk':
-                STATE = 'idle'
+            if state == 'walk':
+                state = 'idle'
                 permission = True
 
     elif module == 'fall_detector':
         permission = True
         if message in ('falling', 'fallen'):
-            STATE = 'fallen'
+            state = 'fallen'
         elif message == 'recovered':
-            STATE = 'idle'
+            state = 'idle'
 
     elif module == 'imitator':
         if message == 'imitate':
-            if STATE == 'imitate':
+            if state in ('imitate',) + (('idle',) if AI else ()):
                 permission = True
+                state = 'imitate'
 
     rospy.logdebug(
-        'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nSTATE IS NOW: %s.' % (
-            module, message, permission, STATE
+        'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nstate IS NOW: %s.' % (
+            module, message, permission, state
         )
     )
-    if _state_old != STATE:
-        rospy.loginfo('{} -> {}'.format(_state_old, STATE))
+    if _state_old != state:
+        rospy.loginfo('{} -> {}'.format(_state_old, state))
     return InformControllerResponse(permission)
 
 
 if __name__ == '__main__':
 
+    print(argv)
+    ap = ArgumentParser()
+    ap.add_argument('-i', '--autoimitate',
+                    help='Switch between moving and imitating automatically',
+                    action='store_true')
+    args, _ = ap.parse_known_args()
+
+    AI = args.autoimitate
+    init_voc_state_speech()
+
     rospy.init_node('controller', log_level=rospy.INFO)
     ic = rospy.Service('inform_controller', InformController, handle_request)
 
@@ -106,13 +136,14 @@ if __name__ == '__main__':
     rospy.on_shutdown(lambda: speech_in_progress and client.cancel_goal())
 
     while not rospy.is_shutdown():
-        if not STATE in ('idle', 'imitate', 'dead'):
+        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)
+        else:
             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/fall_detector.py

@@ -1,6 +1,5 @@
 #! /usr/bin/env python
 import os
-from time import sleep
 
 import rospy
 from naoqi import ALProxy, ALBroker, ALModule
@@ -11,6 +10,7 @@ from controller import inform_controller_factory
 fall_broker = None
 almem = None
 
+
 _inform_controller = inform_controller_factory('fall_detector')
 
 
@@ -27,12 +27,12 @@ class FallDetectorModule(ALModule):
         if not _inform_controller('fallen'):
             return
         self.mp.rest()
-        sleep(2)
+        rospy.Rate(0.5).sleep()
         self.mp.wakeUp()
         pp = ALProxy('ALRobotPosture')
         while not pp.goToPosture('StandInit', 1.0):
             pass
-        sleep(1)
+        rospy.Rate(1).sleep()
         _inform_controller('recovered')
 
 
@@ -51,6 +51,6 @@ if __name__ == "__main__":
                            "on_robot_fallen")
 
     while not rospy.is_shutdown():
-        sleep(0.1)
+        rospy.Rate(10).sleep()
 
     fall_broker.shutdown()