started implementing fall manager

script/controller.py

@@ -23,6 +23,14 @@ def handle_request(r):
             if STATE == 'walk':
                 STATE = 'idle'
             permission = True
+
+    elif module == 'fall_detector':
+        permission = True
+        if message in ('falling', 'fallen'):
+            STATE = 'fallen'
+        elif message == 'recovered':
+            STATE = 'idle'
+
     print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
         module, message, permission, STATE
     )

script/fall_detector.py

@@ -0,0 +1,58 @@
+#! /usr/bin/env python
+import time
+import os
+
+import rospy
+from naoqi import ALProxy
+from naoqi import ALBroker
+from naoqi import ALModule
+
+from teleoperation.srv import InformController
+
+
+fall_broker = None
+almem = None
+
+
+def _inform_controller(what):
+    inform_controller = rospy.ServiceProxy('inform_controller',
+                                           InformController)
+    return inform_controller('fall_detector', what).permission
+
+
+class FallDetectorModule(ALModule):
+
+    def __init__(self, name):
+        ALModule.__init__(self, name)
+
+    def on_robot_falling(self, *_args):
+        print('falling')
+        _inform_controller('falling')
+
+    def on_robot_fallen(self, *_args):
+        print('fallen')
+        _inform_controller('fallen')
+
+
+if __name__ == "__main__":
+    rospy.init_node('fall_detector')
+    rospy.wait_for_service('inform_controller')
+    print 'create broker'
+    fall_broker = ALBroker("fall_broker", "0.0.0.0", 0,
+                           os.environ['NAO_IP'], 9559)
+    print 'create instance'
+    fall_detector = FallDetectorModule("fall_detector")
+    almem = ALProxy("ALMemory")
+    print 'subscribe'
+    almem.subscribeToEvent("robotIsFalling",
+                           "fall_detector",
+                           "on_robot_falling")
+    almem.subscribeToEvent("robotHasFallen",
+                           "fall_detector",
+                           "on_robot_fallen")
+    print 'init success'
+    while not rospy.is_shutdown():
+        time.sleep(1)
+    print
+    print "Interrupted by user, shutting down"
+    fall_detector.broker.shutdown()