refactored the code, fixed config bug

This commit is contained in:
Pavel Lutskov
2019-02-08 16:42:48 +01:00
parent 949f657a75
commit 8861d215c5
11 changed files with 1022 additions and 1064 deletions

View File

@@ -4,7 +4,7 @@ import os
import rospy
from naoqi import ALProxy, ALBroker, ALModule
from masterloop import inform_masterloop_factory
from masterloop import inform_masterloop_factory, mp
fall_broker = None
@@ -18,7 +18,6 @@ class FallDetectorModule(ALModule):
def __init__(self, name):
ALModule.__init__(self, name)
self.mp = ALProxy('ALMotion')
def on_robot_falling(self, *_args):
_inform_masterloop('falling')
@@ -26,9 +25,11 @@ class FallDetectorModule(ALModule):
def on_robot_fallen(self, *_args):
if not _inform_masterloop('fallen'):
return
self.mp.rest()
mp.rest()
rospy.Rate(0.5).sleep()
self.mp.wakeUp()
mp.wakeUp()
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
am.setExpressiveListeningEnabled(False)
pp = ALProxy('ALRobotPosture')
while not pp.goToPosture('StandInit', 1.0):
pass