refactored the code, fixed config bug

script/masterloop.py

@@ -1,4 +1,5 @@
 #! /usr/bin/env python
+import os
 from argparse import ArgumentParser
 
 import rospy
@@ -7,6 +8,7 @@ import actionlib
 from teleoperation.srv import InformMasterloop, InformMasterloopResponse
 from teleoperation.srv import Hands
 from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
+from naoqi import ALProxy
 
 
 # Constants
@@ -32,6 +34,8 @@ state = 'dead'  # Also walk, imitate, fallen, idle
 hand_state = 'closed'
 speech_in_progress = False
 
+mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+
 
 def init_voc_state_speech():
     global VOC_STATE, SPEECH_TRANSITIONS
@@ -144,15 +148,24 @@ if __name__ == '__main__':
     AI = args.autoimitate
     init_voc_state_speech()
 
-    ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
-
     speech = actionlib.SimpleActionClient('speech_server',
                                           RequestSpeechAction)
-
     speech.wait_for_server()
     rospy.loginfo('SPEECH: SERVER THERE')
 
-    rospy.on_shutdown(lambda: speech_in_progress and speech.cancel_goal())
+    mp.wakeUp()
+    mp.moveInit()
+    am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
+    am.setExpressiveListeningEnabled(False)
+
+    ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
+
+    def _shutdown():
+        if speech_in_progress:
+            speech.cancel_goal()
+        mp.rest()
+
+    rospy.on_shutdown(_shutdown)
 
     while not rospy.is_shutdown():
         if state in ('idle', 'imitate', 'dead'):