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

script/walker.py

@@ -2,19 +2,27 @@
 import os
 from time import sleep
 
-from naoqi import ALProxy
-
 import rospy
 import tf
+from naoqi import ALProxy
 
-FW = -0.30
+from teleoperation.srv import InformController
+
+FW = -0.32
 BK = -0.55
 LT = -0.55
 RT = 0.0
 
 
+def _inform_controller(what):
+    inform_controller = rospy.ServiceProxy('inform_controller',
+                                           InformController)
+    return inform_controller('walker', what).permission
+
+
 if __name__ == '__main__':
     rospy.init_node('walker')
+    rospy.wait_for_service('inform_controller')
     ll = tf.TransformListener()
     mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
     mp.wakeUp()
@@ -28,18 +36,22 @@ if __name__ == '__main__':
                                             rospy.Time(0))
         except Exception as e:
             mp.stopMove()
-            # inform_controller('walker', 'stop')
+            _inform_controller('stop')
             continue
-        print trans
+
+        print trans, rot
         # continue
 
-        if BK < trans[2] < FW and LT < trans[0] < RT:
+        if (
+            BK < trans[2] < FW and
+            LT < trans[0] < RT
+            # CW < trans[1] < CC
+        ):
             mp.move(0, 0, 0)
-            # inform_controller('walker', 'stop')
+            _inform_controller('stop')
             continue
 
-        permission = True
-        # permission = inform_controller('walker', 'move')
+        permission = _inform_controller('move')
         if not permission:
             mp.stopMove()
             continue