Interaction of the walker module and main control

script/controller.py

@@ -1,34 +1,36 @@
-from time import sleep
+#! /usr/bin/env python
+import rospy
+
+from teleoperation.srv import InformController, InformControllerResponse
 
 
-handlers = {}
 STATE = 'idle'  # Also walk, imitate and fallen
 
 
-def fall_handler():
-    pass
-
-
-def walk_handler():
-    pass
-
-
-def imitation_handler():
-    pass
-
-
-def handle_transition(new_state):
+def handle_request(r):
+    module = r.module
+    message = r.message
     global STATE
-    if STATE == 'walk':
-        if new_state in ('fallen', 'idle'):
-            STATE = new_state
-    elif STATE == 'fallen':
-        if new_state == 'idle':
-            STATE = new_state
-    elif STATE == 'imitate':
-        STATE = new_state
+
+    if module == 'walker':
+        if message == 'move':
+            if STATE in ('idle', 'walk'):
+                STATE = 'walk'
+                permission = True
+            else:
+                permission = False
+        elif message == 'stop':
+            if STATE == 'walk':
+                STATE = 'idle'
+            permission = True
+    print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
+        module, message, permission, STATE
+    )
+    return InformControllerResponse(permission)
 
 
 if __name__ == '__main__':
-    pass
+    rospy.init_node('controller')
+    ic = rospy.Service('inform_controller', InformController, handle_request)
+    rospy.spin()
     # initialize stuff