diff --git a/script/controller.py b/script/controller.py index 6f9325f..cd0bfd0 100755 --- a/script/controller.py +++ b/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 ) diff --git a/script/fall_detector.py b/script/fall_detector.py new file mode 100755 index 0000000..ac3186a --- /dev/null +++ b/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()