did some work to assemble it together

script/controller.py

@@ -46,8 +46,7 @@ def handle_request(r):
 
     elif module == 'imitator':
         if message == 'imitate':
-            if STATE in ('imitate', 'idle'):
-                STATE = 'imitate'
+            if STATE == 'imitate':
                 permission = True
             else:
                 permission = False
@@ -66,13 +65,15 @@ def handle_request(r):
             else:
                 permission = False
 
-    print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
-        module, message, permission, STATE
+    rospy.loginfo(
+        'Got request from %s to %s. Permission: %s. State is now: %s.' % (
+            module, message, permission, STATE
+        )
     )
     return InformControllerResponse(permission)
 
 
 if __name__ == '__main__':
-    rospy.init_node('controller')
+    rospy.init_node('controller', log_level=rospy.INFO)
     ic = rospy.Service('inform_controller', InformController, handle_request)
     rospy.spin()

script/imitator.py

@@ -27,6 +27,7 @@ if __name__ == '__main__':
         sleep(0.1)
         if not _inform_controller('imitate'):
             continue
+        rospy.loginfo('IMITATOR: ACTIVE')
         for i, eff in enumerate(['L',
                                  'R'], 1):
             try:
@@ -36,18 +37,14 @@ if __name__ == '__main__':
                     rospy.Time(0)
                 )
             except Exception as e:
-                print e
                 continue
+
             sign = 1 if eff == 'L' else -1
             roll = asin(trans[1] /
                         sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
             roll -= sign * radians(25)
             pitch = atan(-trans[2] / abs(trans[0]))
 
-            # sign = 1 if roll > 0 else -1
-            # roll -= sign * radians(10)
-            # print degrees(roll)
-
             mp.setAngles([
                 '{}ShoulderRoll'.format(eff),
                 '{}ShoulderPitch'.format(eff)

script/walker.py

@@ -8,15 +8,32 @@ from naoqi import ALProxy
 
 from controller import inform_controller_factory
 
-FW = -0.32
-BK = -0.55
-LT = -0.55
-RT = 0.0
+
+      #min   #max
+FW =  1.65,  1.45
+BK =  2.20,  2.40
+LT = -0.35, -0.53
+RT =  0.35,  0.53
+
+VMIN = 0.3
+VMAX = 1.0
 
 
 _inform_controller = inform_controller_factory('walker')
 
 
+def _speed(pos, interval):
+    int_dir = 1 if interval[1] > interval[0] else -1
+    if int_dir * (pos - interval[0]) < 0:
+        return 0.0
+    elif int_dir * (pos - interval[1]) > 0:
+        return 1.0
+    else:
+        return (VMAX - VMIN) * abs(pos - interval[0]) / (
+            abs(interval[1] - interval[0])
+        ) + VMIN
+
+
 if __name__ == '__main__':
     rospy.init_node('walker')
     rospy.wait_for_service('inform_controller')
@@ -30,37 +47,39 @@ if __name__ == '__main__':
         sleep(0.3)
         try:
             trans, rot = ll.lookupTransform('Aruco_0_frame',
-                                            'CameraTop_optical_frame',
+                                            'odom',
                                             rospy.Time(0))
         except Exception as e:
             mp.stopMove()
             _inform_controller('stop')
             continue
 
-        print trans, rot
+        # print trans
         # continue
 
-        if (
-            BK < trans[2] < FW and
-            LT < trans[0] < RT
-            # CW < trans[1] < CC
-        ):
-            _inform_controller('stop')
-            mp.move(0, 0, 0)
-            continue
-
         permission = _inform_controller('move')
+
         if not permission:
             mp.stopMove()
             continue
 
-        if trans[2] < BK:  # backwards
-            mp.move(-1, 0, 0)
-        elif FW < trans[2]:  # forwards
-            mp.move(1, 0, 0)
-        elif RT < trans[0]:  # right
-            mp.move(0, -1, 0)
-        elif trans[0] < LT:  # left
-            mp.move(0, 1, 0)
+        movement = [0, 0, 0]
+                               #-1   1  -1   1
+        for i, dr in enumerate((BK, FW, RT, LT)):
+            idx = i // 2
+            sign = 1 if (i % 2) else -1
+            speed = _speed(trans[idx], dr)
+            if speed:
+                movement[idx] = sign * speed
+                break
+
+        if not any(movement):
+            rospy.loginfo('WALKER: STOP')
+            _inform_controller('stop')
+            mp.move(0, 0, 0)
+        else:
+            rospy.loginfo('WALKER: TRANS: {}'.format(trans))
+            rospy.loginfo('WALKER: MOVMT: {}'.format(movement))
+            mp.moveToward(*movement)
 
     mp.rest()