Implemented fall recovery

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