merging iface with other work

launch/fulltest.launch

@@ -3,16 +3,20 @@
 <!--Launch file for Test of teleoperating NAO project-->
 
 <launch>
+    <arg name="autoimitate" default="--no-autoimitate"/>
+    <arg name="controller" default="dumb"/>
+
     <node name="usb_cam" pkg="usb_cam" type="usb_cam_node"/>
     <node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
         output="screen"/>
     <node name="speech_server" pkg="teleoperation" type="speech_server.py"
         output="screen"/>
+
     <node name="masterloop" pkg="teleoperation" type="masterloop.py"
-        output="screen"/>
+        output="screen" args="$(arg autoimitate)"/>
     <node name="hand_ler" pkg="teleoperation" type="hand_ler.py"/>
     <node name="imitator" pkg="teleoperation" type="imitator.py"
-        output="screen"/>
+        output="screen" args="--controller $(arg controller)"/>
     <node name="walker" pkg="teleoperation" type="walker.py"/>
     <node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
 

script/imitator.py

@@ -15,8 +15,26 @@ _inform_masterloop = inform_masterloop_factory('imitator')
 TORSO = False
 
 
+def controller_factory(ctrl):
+    if ctrl == 'nao_cartesian':
+        return lambda my_arm_xyz, side: nao_cart_movement(my_arm_xyz, side)
+
+    if ctrl == 'dumb':
+        cfunc = dumb
+    elif ctrl == 'our_cartesian':
+        cfunc = our_cartesian
+    return lambda my_arm_xyz, side: movement(my_arm_xyz, side, cfunc)
+
+
 if __name__ == '__main__':
     rospy.init_node('imitator')
+    ap = ArgumentParser()
+    ap.add_argument('--controller', default='dumb', const='dumb', nargs='?',
+                    choices=['dumb', 'nao_cartesian', 'our_cartesian'],
+                    help='Choose the controller for arm motions')
+    args, _ = ap.parse_known_args()
+    imitation_cycle = controller_factory(args.controller)
+
     rospy.wait_for_service('inform_masterloop')
 
     ll = tf.TransformListener()
@@ -56,4 +74,4 @@ if __name__ == '__main__':
             my_arm_xyz = np.array(arm) - np.array(a0)
             # rospy.loginfo('{}'.format(my_arm_xyz))
             # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
-            movement(my_arm_xyz, side, dumb)
+            imitation_cycle(my_arm_xyz, side)

script/masterloop.py

@@ -140,9 +140,12 @@ if __name__ == '__main__':
 
     rospy.init_node('masterloop')
     ap = ArgumentParser()
-    ap.add_argument('-i', '--autoimitate',
+    aig = ap.add_mutually_exclusive_group(required=False)
-                    help='Switch between moving and imitating automatically',
+    aig.add_argument('--autoimitate', dest='autoimitate',
                      action='store_true')
+    aig.add_argument('--no-autoimitate', dest='autoimitate',
+                     action='store_false')
+    ap.set_defaults(autoimitate=False)
     args, _ = ap.parse_known_args()
 
     AI = args.autoimitate