59 lines
1.6 KiB
Python
Executable File
59 lines
1.6 KiB
Python
Executable File
#! /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()
|