66 lines
1.8 KiB
Python
Executable File
66 lines
1.8 KiB
Python
Executable File
#! /usr/bin/env python
|
|
"""A module for graceful fall handling.
|
|
|
|
ROS node: `fall_detector`.
|
|
Uses `inform_controller` service.
|
|
|
|
"""
|
|
import os
|
|
|
|
import rospy
|
|
from naoqi import ALProxy, ALBroker, ALModule
|
|
|
|
from masterloop import inform_masterloop_factory, mp
|
|
|
|
|
|
fall_broker = None
|
|
almem = None
|
|
|
|
|
|
_inform_masterloop = inform_masterloop_factory('fall_detector')
|
|
|
|
|
|
class FallDetectorModule(ALModule):
|
|
"""An ALModule responsible for fall handling."""
|
|
|
|
def __init__(self, name):
|
|
ALModule.__init__(self, name)
|
|
|
|
def on_robot_falling(self, *_args):
|
|
_inform_masterloop('falling')
|
|
|
|
def on_robot_fallen(self, *_args):
|
|
if not _inform_masterloop('fallen'): # Seize the control
|
|
return
|
|
mp.rest()
|
|
rospy.Rate(0.5).sleep()
|
|
mp.wakeUp()
|
|
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
|
am.setExpressiveListeningEnabled(False)
|
|
pp = ALProxy('ALRobotPosture')
|
|
while not pp.goToPosture('StandInit', 1.0):
|
|
# Try to stand up indefinetely
|
|
pass
|
|
rospy.Rate(1).sleep()
|
|
_inform_masterloop('recovered') # Release the control
|
|
|
|
|
|
if __name__ == "__main__":
|
|
rospy.init_node('fall_detector')
|
|
rospy.wait_for_service('inform_masterloop')
|
|
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()
|