somehow managed to implement rotation cleanly

script/controller.py

@@ -21,11 +21,11 @@ def inform_controller_factory(who):
 
 
 def handle_request(r):
+    global STATE
     module = r.module
     message = r.message
     _state_old = STATE
     permission = False
-    global STATE
 
     if module == 'walker':
         if message == 'move':

script/walker.py

@@ -10,10 +10,12 @@ from controller import inform_controller_factory
 
 
       #min   #max
-FW =  1.65,  1.45
-BK =  2.20,  2.40
-LT = -0.35, -0.53
-RT =  0.35,  0.53
+FW = -1.65, -1.45
+BK = -2.20, -2.40
+LT =  0.35,  0.53
+RT = -0.35, -0.53
+LR =  0.45,  0.85
+RR = -0.45, -0.85
 
 VMIN = 0.3
 VMAX = 1.0
@@ -39,34 +41,42 @@ if __name__ == '__main__':
     rospy.wait_for_service('inform_controller')
     ll = tf.TransformListener()
     mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
-    mp.wakeUp()
+    # mp.wakeUp()
     mp.moveInit()
     mp.setMoveArmsEnabled(False, False)
 
     while not rospy.is_shutdown():
         sleep(0.3)
         try:
-            trans, rot = ll.lookupTransform('Aruco_0_frame',
-                                            'odom',
-                                            rospy.Time(0))
+            trans, q = ll.lookupTransform('odom',
+                                          'Aruco_0_frame',
+                                          rospy.Time(0))
+            rot = tf.transformations.euler_from_quaternion(q)
+
         except Exception as e:
             mp.stopMove()
             _inform_controller('stop')
             continue
 
         # print trans
+        # print rot
         # continue
 
         movement = [0, 0, 0]
-                               #-1   1  -1   1
-        for i, dr in enumerate((BK, FW, RT, LT)):
+        ref_vec = trans[:2] + rot[2:]
+                               #-1   1  -1   1  -1   1
+        for i, dr in enumerate((BK, FW, RT, LT, RR, LR)):
             idx = i // 2
             sign = 1 if (i % 2) else -1
-            speed = _speed(trans[idx], dr)
+            speed = _speed(ref_vec[idx], dr)
             if speed:
                 movement[idx] = sign * speed
                 break
 
+        rospy.loginfo('WALKER: TRANS: {}'.format(trans))
+        rospy.loginfo('WALKER: ROTTN: {}'.format(rot))
+        rospy.loginfo('WALKER: MOVMT: {}\n'.format(movement))
+
         if not any(movement):
             rospy.logdebug('WALKER: STOP')
             _inform_controller('stop')
@@ -77,10 +87,8 @@ if __name__ == '__main__':
 
         if not permission:
             mp.stopMove()
-            continue
-
-        rospy.loginfo('WALKER: TRANS: {}'.format(trans))
-        rospy.loginfo('WALKER: MOVMT: {}'.format(movement))
-        mp.moveToward(*movement)
+        else:
+            # mp.moveToward(*movement)  # DON'T DELETE THIS ONE
+            pass
 
     mp.rest()