Implemented fall recovery

.gitignore

@@ -1,3 +1,6 @@
+# Python stuff
+*.pyc
+
 # Vim stuff
 .*.sw*
 

CMakeLists.txt

@@ -31,6 +31,7 @@ include_directories(
 catkin_install_python(PROGRAMS
     ./script/controller.py
     ./script/walker.py
+    ./script/fall_manager.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

script/controller.py

@@ -7,6 +7,19 @@ from teleoperation.srv import InformController, InformControllerResponse
 STATE = 'idle'  # Also walk, imitate and fallen
 
 
+def inform_controller_factory(who):
+    def inform_controller(what):
+        try:
+            inform_controller = rospy.ServiceProxy('inform_controller',
+                                                   InformController)
+            perm = inform_controller(who, what).permission
+        except rospy.service.ServiceException:
+            rospy.signal_shutdown('Controller is dead')
+            perm = False
+        return perm
+    return inform_controller
+
+
 def handle_request(r):
     module = r.module
     message = r.message
@@ -41,4 +54,3 @@ if __name__ == '__main__':
     rospy.init_node('controller')
     ic = rospy.Service('inform_controller', InformController, handle_request)
     rospy.spin()
-    # initialize stuff

script/fall_detector.py

@@ -1,58 +1,56 @@
 #! /usr/bin/env python
-import time
 import os
+from time import sleep
 
 import rospy
-from naoqi import ALProxy
-from naoqi import ALBroker
-from naoqi import ALModule
+from naoqi import ALProxy, ALBroker, ALModule
 
-from teleoperation.srv import InformController
+from controller import inform_controller_factory
 
 
 fall_broker = None
 almem = None
 
-
-def _inform_controller(what):
-    inform_controller = rospy.ServiceProxy('inform_controller',
-                                           InformController)
-    return inform_controller('fall_detector', what).permission
+_inform_controller = inform_controller_factory('fall_detector')
 
 
 class FallDetectorModule(ALModule):
 
     def __init__(self, name):
         ALModule.__init__(self, name)
+        self.mp = ALProxy('ALMotion')
 
     def on_robot_falling(self, *_args):
-        print('falling')
         _inform_controller('falling')
 
     def on_robot_fallen(self, *_args):
-        print('fallen')
-        _inform_controller('fallen')
+        if not _inform_controller('fallen'):
+            return
+        self.mp.rest()
+        sleep(2)
+        self.mp.wakeUp()
+        pp = ALProxy('ALRobotPosture')
+        while not pp.goToPosture('StandInit', 1.0):
+            pass
+        sleep(1)
+        _inform_controller('recovered')
 
 
 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()
+        sleep(0.1)
+
+    fall_broker.shutdown()

script/walker.py

@@ -6,7 +6,7 @@ import rospy
 import tf
 from naoqi import ALProxy
 
-from teleoperation.srv import InformController
+from controller import inform_controller_factory
 
 FW = -0.32
 BK = -0.55
@@ -14,10 +14,7 @@ LT = -0.55
 RT = 0.0
 
 
-def _inform_controller(what):
-    inform_controller = rospy.ServiceProxy('inform_controller',
-                                           InformController)
-    return inform_controller('walker', what).permission
+_inform_controller = inform_controller_factory('walker')
 
 
 if __name__ == '__main__':
@@ -36,7 +33,7 @@ if __name__ == '__main__':
                                             rospy.Time(0))
         except Exception as e:
             mp.stopMove()
-            _inform_controller('stop')
+            print _inform_controller('stop')
             continue
 
         print trans, rot
@@ -55,6 +52,7 @@ if __name__ == '__main__':
         if not permission:
             mp.stopMove()
             continue
+
         if trans[2] < BK:  # backwards
             mp.move(-1, 0, 0)
         elif FW < trans[2]:  # forwards