From 36f787087ce328e6b5abcc7065984f70aa70ed0f Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@tum.de>
Date: Mon, 4 Feb 2019 16:23:48 +0100
Subject: [PATCH] renamed controller to masterloop

---
 CMakeLists.txt                                |  4 +-
 launch/fulltest.launch                        |  2 +-
 script/calibrator.py                          |  4 +-
 script/cartesian_controller.py                | 57 +++++++++++--------
 script/fall_detector.py                       | 12 ++--
 script/imitator.py                            |  8 +--
 script/{controller.py => masterloop.py}       | 22 +++----
 script/walker.py                              | 12 ++--
 ...ormController.srv => InformMasterloop.srv} |  0
 9 files changed, 64 insertions(+), 57 deletions(-)
 rename script/{controller.py => masterloop.py} (88%)
 rename srv/{InformController.srv => InformMasterloop.srv} (100%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e31f40c..6f6d813 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,7 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_service_files(
     DIRECTORY srv
     FILES
-    InformController.srv
+    InformMasterloop.srv
     Hands.srv
 )
 
@@ -41,7 +41,7 @@ include_directories(
 
 catkin_install_python(PROGRAMS
     ./script/calibrator.py
-    ./script/controller.py
+    ./script/masterloop.py
     ./script/walker.py
     ./script/fall_detector.py
     ./script/imitator.py
diff --git a/launch/fulltest.launch b/launch/fulltest.launch
index 86d2931..e65b32d 100644
--- a/launch/fulltest.launch
+++ b/launch/fulltest.launch
@@ -8,7 +8,7 @@
         output="screen"/>
     <node name="speech_server" pkg="teleoperation" type="speech_server.py"
         output="screen"/>
-    <node name="controller" pkg="teleoperation" type="controller.py"
+    <node name="masterloop" pkg="teleoperation" type="masterloop.py"
         output="screen"/>
     <node name="hand_ler" pkg="teleoperation" type="hand_ler.py"/>
     <node name="imitator" pkg="teleoperation" type="imitator.py"
diff --git a/script/calibrator.py b/script/calibrator.py
index 146e57c..32b4f97 100755
--- a/script/calibrator.py
+++ b/script/calibrator.py
@@ -9,11 +9,11 @@ import actionlib
 import numpy as np
 from naoqi import ALProxy
 
-from controller import inform_controller_factory
+from masterloop import inform_masterloop_factory
 from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
 
 
-_inform_controller = inform_controller_factory('calibrator')
+_inform_masterloop = inform_masterloop_factory('calibrator')
 
 
 def calibration():
diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py
index ec584d0..6988774 100755
--- a/script/cartesian_controller.py
+++ b/script/cartesian_controller.py
@@ -1,19 +1,20 @@
 #! /usr/bin/env python
-import os 
+import os
 
 import rospy
 import numpy as np
 
-import sys
 from naoqi import ALProxy
 import motion
 
-motionProxy = 0
+
+mp = None
+
 
 def get_transform(joint):
     frame = motion.FRAME_TORSO
     useSensorValues = True
-    result = motionProxy.getTransform(joint,frame,useSensorValues)
+    result = mp.getTransform(joint,frame,useSensorValues)
     result = np.matrix(result)
     print result
     result = np.reshape(result, (4,4))
@@ -25,15 +26,15 @@ def cartesian_position(joint):
     print 'function'
     frame = motion.FRAME_TORSO
     useSensorValues = True
-    result = motionProxy.getPosition(joint, frame, useSensorValues)
+    result = mp.getPosition(joint, frame, useSensorValues)
     #print result
     return np.array(result[:3])
 
 
 def jacobian():
-
-    # get current positions/ accordint to control figure these values should actually come from the
-    # integration step in the previous first control loop
+    # get current positions
+    # according to control figure these values should actually come
+    # from the integration step in the previous first control loop
 
     end_position = cartesian_position('LArm')
 
@@ -47,21 +48,28 @@ def jacobian():
     x_axis = np.array([[1, 0, 0, 1]]).T
     y_axis = np.array([[0, 1, 0, 1]]).T
     z_axis = np.array([[0, 0, 1, 1]]).T
-    
+
     shoulder_axis = get_transform('LShoulderPitch').dot(y_axis)
     bicep_axis = get_transform('LShoulderRoll').dot(z_axis)
     elbow_axis = get_transform('LElbowYaw').dot(x_axis)
     forearm_axis = get_transform('LElbowRoll').dot(z_axis)
 
     # get basis vectors of jacobian
-    
-    shoulder_basis = np.cross(shoulder_axis[:3].flatten(), end_position - shoulder_position)
-    bicep_basis = np.cross(bicep_axis[:3].flatten(), end_position - bicep_position)
-    elbow_basis = np.cross(elbow_axis[:3].flatten(), end_position - elbow_position)
-    forearm_basis = np.cross(forearm_axis[:3].flatten(), end_position - forearm_position)
+
+    shoulder_basis = np.cross(shoulder_axis[:3].flatten(),
+                              end_position - shoulder_position)
+    bicep_basis = np.cross(bicep_axis[:3].flatten(),
+                           end_position - bicep_position)
+    elbow_basis = np.cross(elbow_axis[:3].flatten(),
+                           end_position - elbow_position)
+    forearm_basis = np.cross(forearm_axis[:3].flatten(),
+                             end_position - forearm_position)
 
     # build jacobian matrix
-    jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T
+    jacobian = np.concatenate(
+        [shoulder_basis, bicep_basis, elbow_basis, forearm_basis],
+        axis=0
+    ).T
 
     return jacobian
 
@@ -70,7 +78,7 @@ def pseudo_inverse(jacobian):
     return np.linalg.pinv(jacobian)
 
 
-def reference_generator(p_d)
+def reference_generator(p_d):
 
     # calculate jacobian
 
@@ -83,20 +91,19 @@ def reference_generator(p_d)
     return
 
 
-def movement(e)
+def movement(e):
 
     # scale joint states with matrix K
 
     # add desired joint speed
 
-    # move robot arm    
+    # move robot arm
 
     return
- 
+
 
 if __name__ == '__main__':
-
-    motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
+    mp = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
     jacob = jacobian()
     print jacob
     jacob = pseudo_inverse(jacob)
@@ -105,7 +112,7 @@ if __name__ == '__main__':
     # given new desired coordinates
 
     e = 1;
-    """ 
+    """
     while e bigger some value
 
         # run reference generator to get desired joint postion and speed
@@ -113,8 +120,8 @@ if __name__ == '__main__':
         # subtract current joint states
 
         # movement
- 
-    """    
-    #rospy.init_node('cartesian_controller')
+
+    """
+    #rospy.init_node('cartesian_masterloop')
     #rospy.spin()
 
diff --git a/script/fall_detector.py b/script/fall_detector.py
index 360977a..58de4cc 100755
--- a/script/fall_detector.py
+++ b/script/fall_detector.py
@@ -4,14 +4,14 @@ import os
 import rospy
 from naoqi import ALProxy, ALBroker, ALModule
 
-from controller import inform_controller_factory
+from masterloop import inform_masterloop_factory
 
 
 fall_broker = None
 almem = None
 
 
-_inform_controller = inform_controller_factory('fall_detector')
+_inform_masterloop = inform_masterloop_factory('fall_detector')
 
 
 class FallDetectorModule(ALModule):
@@ -21,10 +21,10 @@ class FallDetectorModule(ALModule):
         self.mp = ALProxy('ALMotion')
 
     def on_robot_falling(self, *_args):
-        _inform_controller('falling')
+        _inform_masterloop('falling')
 
     def on_robot_fallen(self, *_args):
-        if not _inform_controller('fallen'):
+        if not _inform_masterloop('fallen'):
             return
         self.mp.rest()
         rospy.Rate(0.5).sleep()
@@ -33,12 +33,12 @@ class FallDetectorModule(ALModule):
         while not pp.goToPosture('StandInit', 1.0):
             pass
         rospy.Rate(1).sleep()
-        _inform_controller('recovered')
+        _inform_masterloop('recovered')
 
 
 if __name__ == "__main__":
     rospy.init_node('fall_detector')
-    rospy.wait_for_service('inform_controller')
+    rospy.wait_for_service('inform_masterloop')
     fall_broker = ALBroker("fall_broker", "0.0.0.0", 0,
                            os.environ['NAO_IP'], 9559)
     fall_detector = FallDetectorModule("fall_detector")
diff --git a/script/imitator.py b/script/imitator.py
index 59f5677..f525c01 100755
--- a/script/imitator.py
+++ b/script/imitator.py
@@ -9,10 +9,10 @@ import tf
 import numpy as np
 from naoqi import ALProxy
 
-from controller import inform_controller_factory
+from masterloop import inform_masterloop_factory
 
 
-_inform_controller = inform_controller_factory('imitator')
+_inform_masterloop = inform_masterloop_factory('imitator')
 
 
 _FRAME_TORSO = 0
@@ -26,7 +26,7 @@ def _elbow(arm_):
 
 if __name__ == '__main__':
     rospy.init_node('imitator')
-    rospy.wait_for_service('inform_controller')
+    rospy.wait_for_service('inform_masterloop')
     ll = tf.TransformListener()
     am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
     am.setExpressiveListeningEnabled(False)
@@ -43,7 +43,7 @@ if __name__ == '__main__':
 
     while not rospy.is_shutdown():
         sleep(0.1)
-        if not _inform_controller('imitate'):
+        if not _inform_masterloop('imitate'):
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
 
diff --git a/script/controller.py b/script/masterloop.py
similarity index 88%
rename from script/controller.py
rename to script/masterloop.py
index ecd1974..7a7ab93 100755
--- a/script/controller.py
+++ b/script/masterloop.py
@@ -4,7 +4,7 @@ from argparse import ArgumentParser
 import rospy
 import actionlib
 
-from teleoperation.srv import InformController, InformControllerResponse
+from teleoperation.srv import InformMasterloop, InformMasterloopResponse
 from teleoperation.srv import Hands
 from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
 
@@ -79,17 +79,17 @@ def speech_done_cb(_, result):
     speech_in_progress = False
 
 
-def inform_controller_factory(who):
-    def inform_controller(what):
+def inform_masterloop_factory(who):
+    def inform_masterloop(what):
         try:
-            inform_controller = rospy.ServiceProxy('inform_controller',
-                                                   InformController)
-            perm = inform_controller(who, what).permission
+            inform_masterloop = rospy.ServiceProxy('inform_masterloop',
+                                                   InformMasterloop)
+            perm = inform_masterloop(who, what).permission
         except rospy.service.ServiceException:
-            rospy.signal_shutdown('Controller is dead')
+            rospy.signal_shutdown('Masterloop is dead')
             perm = False
         return perm
-    return inform_controller
+    return inform_masterloop
 
 
 def handle_request(r):
@@ -129,12 +129,12 @@ def handle_request(r):
     )
     if _state_old != state:
         rospy.loginfo('{} -> {}'.format(_state_old, state))
-    return InformControllerResponse(permission)
+    return InformMasterloopResponse(permission)
 
 
 if __name__ == '__main__':
 
-    rospy.init_node('controller')
+    rospy.init_node('masterloop')
     ap = ArgumentParser()
     ap.add_argument('-i', '--autoimitate',
                     help='Switch between moving and imitating automatically',
@@ -144,7 +144,7 @@ if __name__ == '__main__':
     AI = args.autoimitate
     init_voc_state_speech()
 
-    ic = rospy.Service('inform_controller', InformController, handle_request)
+    ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
 
     speech = actionlib.SimpleActionClient('speech_server',
                                           RequestSpeechAction)
diff --git a/script/walker.py b/script/walker.py
index be85308..9c54735 100755
--- a/script/walker.py
+++ b/script/walker.py
@@ -7,7 +7,7 @@ import rospy
 import tf
 from naoqi import ALProxy
 
-from controller import inform_controller_factory
+from masterloop import inform_masterloop_factory
 
 
 FW = None
@@ -40,7 +40,7 @@ def global_init():
     RR = n_way(cz, x['rr'], 2), x['rr']
 
 
-_inform_controller = inform_controller_factory('walker')
+_inform_masterloop = inform_masterloop_factory('walker')
 
 
 def _speed(pos, interval):
@@ -58,7 +58,7 @@ def _speed(pos, interval):
 if __name__ == '__main__':
     rospy.init_node('walker')
     global_init()
-    rospy.wait_for_service('inform_controller')
+    rospy.wait_for_service('inform_masterloop')
     ll = tf.TransformListener()
     mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
     mp.wakeUp()
@@ -75,7 +75,7 @@ if __name__ == '__main__':
 
         except Exception as e:
             mp.stopMove()
-            _inform_controller('stop')
+            _inform_masterloop('stop')
             continue
 
         movement = [0, 0, 0]
@@ -95,12 +95,12 @@ if __name__ == '__main__':
 
         if not any(movement):
             rospy.logdebug('WALKER: STOP')
-            _inform_controller('stop')
+            _inform_masterloop('stop')
             # mp.move(0, 0, 0)
             mp.stopMove()
             continue
 
-        permission = _inform_controller('move')
+        permission = _inform_masterloop('move')
 
         if not permission:
             mp.stopMove()
diff --git a/srv/InformController.srv b/srv/InformMasterloop.srv
similarity index 100%
rename from srv/InformController.srv
rename to srv/InformMasterloop.srv