From 2644ab95fac2cd101e407133c37a07a5aff3af38 Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@gmail.com>
Date: Sat, 9 Feb 2019 16:21:57 +0100
Subject: [PATCH] arguments for scripts(maybe useful?)

---
 launch/fulltest.launch |  8 ++++++--
 script/imitator.py     | 20 +++++++++++++++++++-
 script/masterloop.py   |  9 ++++++---
 3 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/launch/fulltest.launch b/launch/fulltest.launch
index f94ed1f..980f19e 100644
--- a/launch/fulltest.launch
+++ b/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"/>
 
diff --git a/script/imitator.py b/script/imitator.py
index 7c5f3a5..9edda1c 100755
--- a/script/imitator.py
+++ b/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)
diff --git a/script/masterloop.py b/script/masterloop.py
index 26ea8c6..cf6e5d6 100755
--- a/script/masterloop.py
+++ b/script/masterloop.py
@@ -140,9 +140,12 @@ if __name__ == '__main__':
 
     rospy.init_node('masterloop')
     ap = ArgumentParser()
-    ap.add_argument('-i', '--autoimitate',
-                    help='Switch between moving and imitating automatically',
-                    action='store_true')
+    aig = ap.add_mutually_exclusive_group(required=False)
+    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