diff --git a/.gitignore b/.gitignore index 42b4aab..3dfb892 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ +# Python stuff +*.pyc + # Vim stuff .*.sw* diff --git a/CMakeLists.txt b/CMakeLists.txt index 40331b6..ed6f87d 100644 --- a/CMakeLists.txt +++ b/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} ) diff --git a/script/controller.py b/script/controller.py index cd0bfd0..3eb3696 100755 --- a/script/controller.py +++ b/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 diff --git a/script/fall_detector.py b/script/fall_detector.py index ac3186a..03c1d4d 100755 --- a/script/fall_detector.py +++ b/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() diff --git a/script/walker.py b/script/walker.py index 534e8b4..89382f8 100755 --- a/script/walker.py +++ b/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