57 lines
1.5 KiB
Python
Executable File
57 lines
1.5 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import os
|
|
|
|
import rospy
|
|
from naoqi import ALProxy, ALBroker, ALModule
|
|
|
|
from controller import inform_controller_factory
|
|
|
|
|
|
fall_broker = None
|
|
almem = None
|
|
|
|
|
|
_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):
|
|
_inform_controller('falling')
|
|
|
|
def on_robot_fallen(self, *_args):
|
|
if not _inform_controller('fallen'):
|
|
return
|
|
self.mp.rest()
|
|
rospy.Rate(0.5).sleep()
|
|
self.mp.wakeUp()
|
|
pp = ALProxy('ALRobotPosture')
|
|
while not pp.goToPosture('StandInit', 1.0):
|
|
pass
|
|
rospy.Rate(1).sleep()
|
|
_inform_controller('recovered')
|
|
|
|
|
|
if __name__ == "__main__":
|
|
rospy.init_node('fall_detector')
|
|
rospy.wait_for_service('inform_controller')
|
|
fall_broker = ALBroker("fall_broker", "0.0.0.0", 0,
|
|
os.environ['NAO_IP'], 9559)
|
|
fall_detector = FallDetectorModule("fall_detector")
|
|
almem = ALProxy("ALMemory")
|
|
almem.subscribeToEvent("robotIsFalling",
|
|
"fall_detector",
|
|
"on_robot_falling")
|
|
almem.subscribeToEvent("robotHasFallen",
|
|
"fall_detector",
|
|
"on_robot_fallen")
|
|
|
|
while not rospy.is_shutdown():
|
|
rospy.Rate(10).sleep()
|
|
|
|
fall_broker.shutdown()
|