refactored the code, fixed config bug

script/imitator.py

@@ -1,12 +1,10 @@
 #! /usr/bin/env python
-import os
 
 import rospy
 import tf
-from naoqi import ALProxy
 
 from masterloop import inform_masterloop_factory
-from controller import movement, dumb, mp
+from controller import movement, dumb, our_cartesian
 
 
 _inform_masterloop = inform_masterloop_factory('imitator')
@@ -21,27 +19,22 @@ if __name__ == '__main__':
 
     ll = tf.TransformListener()
 
-    am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
-    am.setExpressiveListeningEnabled(False)
-
-    mp.wakeUp()
-
     while not rospy.is_shutdown():
         rospy.Rate(10).sleep()
         if not _inform_masterloop('imitate'):
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
 
-        if TORSO:
-            try:
-                _, q = ll.lookupTransform('odom',
-                                          'Aruco_0_frame',
-                                          rospy.Time(0))
-                rot = tf.transformations.euler_from_quaternion(q)
-                mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
-                             [-rot[1], -rot[1]], 0.3)
-            except Exception as e:
-                rospy.logwarn(e)
+        # if TORSO:
+            # try:
+                # _, q = ll.lookupTransform('odom',
+                                          # 'Aruco_0_frame',
+                                          # rospy.Time(0))
+                # rot = tf.transformations.euler_from_quaternion(q)
+                # mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
+                             # [-rot[1], -rot[1]], 0.3)
+            # except Exception as e:
+                # rospy.logwarn(e)
 
         for i, side in enumerate(['L', 'R'], 1):
             try:
@@ -54,6 +47,4 @@ if __name__ == '__main__':
                 rospy.logwarn(e)
                 continue
 
-            movement(my_arm_xyz, side, dumb)
-
-    mp.rest()
+            movement(my_arm_xyz, side, our_cartesian)