commit 013997fb2594ec3f1ab973c765fe675149592310
Author: fl <flier@techfak.uni-bielefeld.de>
Date:   Wed Dec 2 17:48:01 2015 +0100

    Next try

diff --git a/client/python/hlrc_client/simple-robot-gaze.py b/client/python/hlrc_client/simple-robot-gaze.py
deleted file mode 100644
index 128ee2b..0000000
--- a/client/python/hlrc_client/simple-robot-gaze.py
+++ /dev/null
@@ -1,265 +0,0 @@
-#!/usr/bin/python
-
-__author__ = 'fl@techfak'
-
-# STD IMPORTS
-import sys
-import time
-import signal
-import logging
-import operator
-
-# HLRC
-from hlrc_client import *
-
-# ROS IMPORTS
-import rospy
-import roslib
-from std_msgs.msg import Header
-from std_msgs.msg import String
-from people_msgs.msg import Person
-from people_msgs.msg import People
-
-
-class RobotDriver():
-    """
-    This class holds the robot controller.
-    Provides better encapsulation though...
-    """
-    def __init__(self, _mw, _outscope):
-        print(">>> Initializing Robot Controller")
-        self.mw               = _mw
-        self.outscope         = _outscope
-        self.robot_controller = RobotController(self.mw, self.outscope, logging.INFO)
-
-
-class GazeController():
-    """
-    The GazeController receives person messages (ROS) and derives
-    the nearest person identified. Based on this, the robot's
-    joint angle target's are derived using the transformation
-    class below
-    """
-    def __init__(self, _robot_controller, _affine_transform, _inscope):
-        print(">>> Initializing Gaze Controller")
-        self.run      = True
-        self.inscope  = _inscope
-        self.rc       = _robot_controller
-        self.at       = _affine_transform
-        self.nearest_person_x = 0.0
-        self.nearest_person_y = 0.0
-        signal.signal(signal.SIGINT, self.signal_handler)
-
-    def signal_handler(self, signal, frame):
-            print ">>> ROS is about to exit (signal %s)..." % str(signal)
-            self.run = False
-
-    def people_callback(self, ros_data):
-        # Determine the nearest person
-        idx = -1
-        max_distance = {}
-        for person in ros_data.people:
-            idx += 1
-            max_distance[str(idx)] = person.position.z
-        print ">> Persons found {idx, distance}: ", max_distance
-        sort = sorted(max_distance.items(), key=operator.itemgetter(1), reverse=True)
-        print ">> Nearest Face: ", sort
-        print ">> Index: ", sort[0][0]
-        print ">> Distance in pixels: ", sort[0][1]
-        self.nearest_person_x = ros_data.people[int(sort[0][0])].position.x
-        self.nearest_person_y = ros_data.people[int(sort[0][0])].position.y
-        print ">> Position in pixels x:", self.nearest_person_x
-        print ">> Position in pixels y:", self.nearest_person_y
-        point = [self.nearest_person_x, self.nearest_person_y]
-        # Derive coordinate mapping
-        angles = self.at.derive_mapping_coords(point)
-        print "----------------"
-        if angles is not None:
-            # Set the robot gaze
-            g = RobotGaze()
-            g.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE
-            g.pan = angles[0]
-            g.tilt = angles[1]
-            print ">> Sending Gaze Type:", g
-            self.rc.robot_controller.set_gaze_target(g, False)
-
-    def run_subscriber(self):
-        print(">>> Initializing Gaze Subscriber")
-        person_subscriber = rospy.Subscriber(self.inscope, People, self.people_callback, queue_size=1)
-        while self.run:
-            time.sleep(1)
-        person_subscriber.unregister()
-        print ">>> Deactivating ROS Subscriber"
-
-    def derive_gaze_angle(self):
-        pass
-
-
-class AffineTransform:
-    """
-    Derives the transformation between screen
-    coordinates in pixels and joint axis angles in degree.
-    """
-    def __init__(self):
-        print(">>> Initializing Affine Transform")
-        # Target ---> The ones you want to map to
-        self.target0 = [1.0, 1.0]
-        self.target1 = [1.0, 1.0]
-        self.target2 = [1.0, 1.0]
-        self.target3 = [1.0, 1.0]
-
-        # Origin ---> The ones that are mapped to [target0, target1, target2, target3]
-        self.origin0 = [1.0, 1.0]
-        self.origin1 = [1.0, 1.0]
-        self.origin2 = [1.0, 1.0]
-        self.origin3 = [1.0, 1.0]
-
-        # Divider
-        self.divider = 1.0
-
-        # Calculated and mapped Coordinates
-        mappedCoords = [1.0, 1.0]
-
-        # Affine transformation coefficients
-        self.An = 1.0
-        self.Bn = 1.0
-        self.Cn = 1.0
-        self.Dn = 1.0
-        self.En = 1.0
-        self.Fn = 1.0
-
-        # Test coord
-        self.test = [1.0, 1.0]
-
-    def set_coords(self):
-
-        # This is the target coordinate system
-        # Upper left corner
-        self.target0[0] = -45.0
-        self.target0[1] = 45.0
-
-        # Lower left corner
-        self.target1[0] = -45.0
-        self.target1[1] = -45.0
-
-        # Upper right corner
-        self.target2[0] = 45.0
-        self.target2[1] = 45.0
-
-        # Lower right corner
-        self.target3[0] = 45.0
-        self.target3[1] = -45.0
-
-        # This is the origin system, is mapped to [t0,t1,t2,t3]
-        # Upper left corner
-        self.origin0[0] = 0.0
-        self.origin0[1] = 0.0
-
-        # Lower left corner
-        self.origin1[0] = 0.0
-        self.origin1[1] = 240.0
-
-         # Upper right corner
-        self.origin2[0] = 320.0
-        self.origin2[1] = 0.0
-
-         # Lower right corner
-        self.origin3[0] = 320.0
-        self.origin3[1] = 240.0
-
-        # And finally the test coordinate
-        self.test[0] = 512.0
-        self.test[1] = 384.0
-
-    def calculate_divider(self):
-        result = ((self.origin0[0] - self.origin2[0]) * (self.origin1[1] - self.origin2[1])) - \
-                 ((self.origin1[0] - self.origin2[0]) * (self.origin0[1] - self.origin2[1]))
-
-        if result == 0.0:
-            print(">> Divider is ZERO - Check your Coordinates?")
-            sys.exit(1)
-        else:
-            self.divider = result
-            print(">> Divider " + str(self.divider))
-            self.calculateAn()
-            self.calculateBn()
-            self.calculateCn()
-            self.calculateDn()
-            self.calculateEn()
-            self.calculateFn()
-
-        return result
-
-    def calculateAn(self):
-        result = ((self.target0[0] - self.target2[0]) * (self.origin1[1] - self.origin2[1])) - \
-                 ((self.target1[0] - self.target2[0]) * (self.origin0[1] - self.origin2[1]))
-        self.An = result
-        print(">> An " + str(self.An))
-        return result
-
-    def calculateBn(self):
-        result = ((self.origin0[0] - self.origin2[0]) * (self.target1[0] - self.target2[0])) - \
-                 ((self.target0[0] - self.target2[0]) * (self.origin1[0] - self.origin2[0]))
-        self.Bn = result
-        print(">> Bn " + str(self.Bn))
-        return result
-
-    def calculateCn(self):
-        result = (self.origin2[0] * self.target1[0] - self.origin1[0] * self.target2[0]) * self.origin0[1] + \
-                 (self.origin0[0] * self.target2[0] - self.origin2[0] * self.target0[0]) * self.origin1[1] + \
-                 (self.origin1[0] * self.target0[0] - self.origin0[0] * self.target1[0]) * self.origin2[1]
-        self.Cn = result
-        print(">> Cn " + str(self.Cn))
-        return result
-
-    def calculateDn(self):
-        result = ((self.target0[1] - self.target2[1]) * (self.origin1[1] - self.origin2[1])) - \
-                 ((self.target1[1] - self.target2[1]) * (self.origin0[1] - self.origin2[1]))
-        self.Dn = result
-        print(">> Dn " + str(self.Dn))
-        return result
-
-    def calculateEn(self):
-        result = ((self.origin0[0] - self.origin2[0]) * (self.target1[1] - self.target2[1])) - \
-                 ((self.target0[1] - self.target2[1]) * (self.origin1[0] - self.origin2[0]))
-        self.En = result
-        print(">> En " + str(self.En))
-        return result
-
-    def calculateFn(self):
-        result = (self.origin2[0] * self.target1[1] - self.origin1[0] * self.target2[1]) * self.origin0[1] + \
-                 (self.origin0[0] * self.target2[1] - self.origin2[0] * self.target0[1]) * self.origin1[1] + \
-                 (self.origin1[0] * self.target0[1] - self.origin0[0] * self.target1[1]) * self.origin2[1]
-        self.Fn = result
-        print(">> Fn " + str(self.Fn))
-        return result
-
-    def derive_mapping_coords(self, point):
-        # r->x = ((matrixPtr->An * ad->x) + (matrixPtr->Bn * ad->y) + matrixPtr->Cn) / matrixPtr->Divider
-        # r->y = ((matrixPtr->Dn * ad->x) + (matrixPtr->En * ad->y) + matrixPtr->Fn) / matrixPtr->Divider
-        if self.divider != 0.0:
-            x = ((self.An * point[0]) + (self.Bn * point[1]) + self.Cn) / self.divider
-            y = ((self.Dn * point[0]) + (self.En * point[1]) + self.Fn) / self.divider
-            result = [x, y]
-            # print ">> Current Coordinate (Face):", point
-            print ">> x-pixels were mapped to angle: %s \n>> y-pixels were mapped to angle: %s" % (str(x), str(y))
-            return result
-        else:
-            return None
-
-
-def runner(arguments):
-    if len(arguments) != 3:
-        print(">>> Usage: simple-robot-gaze.py <inscope 'persons_scope'> <outscope 'gaze_target_scope'>\n\n")
-        sys.exit(1)
-
-    rd = RobotDriver("ROS", sys.argv[2])
-    at = AffineTransform()
-    at.set_coords()
-    at.calculate_divider()
-    gc = GazeController(rd, at, sys.argv[1])
-    gc.run_subscriber()
-
-if __name__ == '__main__':
-    runner(sys.argv)
-
diff --git a/client/python/hlrc_client/simple_robot_gaze.py b/client/python/hlrc_client/simple_robot_gaze.py
new file mode 100644
index 0000000..ccc3c9c
--- /dev/null
+++ b/client/python/hlrc_client/simple_robot_gaze.py
@@ -0,0 +1,265 @@
+#!/usr/bin/python
+
+__author__ = 'fl@techfak'
+
+# STD IMPORTS
+import sys
+import time
+import signal
+import logging
+import operator
+
+# HLRC
+from hlrc_client import *
+
+# ROS IMPORTS
+import rospy
+import roslib
+from std_msgs.msg import Header
+from std_msgs.msg import String
+from people_msgs.msg import Person
+from people_msgs.msg import People
+
+
+class RobotDriver():
+    """
+    This class holds the robot controller.
+    Provides better encapsulation though...
+    """
+    def __init__(self, _mw, _outscope):
+        print(">>> Initializing Robot Controller")
+        self.mw               = _mw
+        self.outscope         = _outscope
+        self.robot_controller = RobotController(self.mw, self.outscope, logging.INFO)
+
+
+class GazeController():
+    """
+    The GazeController receives person messages (ROS) and derives
+    the nearest person identified. Based on this, the robot's
+    joint angle target's are derived using the transformation
+    class below
+    """
+    def __init__(self, _robot_controller, _affine_transform, _inscope):
+        print(">>> Initializing Gaze Controller")
+        self.run      = True
+        self.inscope  = _inscope
+        self.rc       = _robot_controller
+        self.at       = _affine_transform
+        self.nearest_person_x = 0.0
+        self.nearest_person_y = 0.0
+        signal.signal(signal.SIGINT, self.signal_handler)
+
+    def signal_handler(self, signal, frame):
+            print ">>> ROS is about to exit (signal %s)..." % str(signal)
+            self.run = False
+
+    def people_callback(self, ros_data):
+        # Determine the nearest person
+        idx = -1
+        max_distance = {}
+        for person in ros_data.people:
+            idx += 1
+            max_distance[str(idx)] = person.position.z
+        print ">> Persons found {idx, distance}: ", max_distance
+        sort = sorted(max_distance.items(), key=operator.itemgetter(1), reverse=True)
+        print ">> Nearest Face: ", sort
+        print ">> Index: ", sort[0][0]
+        print ">> Distance in pixels: ", sort[0][1]
+        self.nearest_person_x = ros_data.people[int(sort[0][0])].position.x
+        self.nearest_person_y = ros_data.people[int(sort[0][0])].position.y
+        print ">> Position in pixels x:", self.nearest_person_x
+        print ">> Position in pixels y:", self.nearest_person_y
+        point = [self.nearest_person_x, self.nearest_person_y]
+        # Derive coordinate mapping
+        angles = self.at.derive_mapping_coords(point)
+        print "----------------"
+        if angles is not None:
+            # Set the robot gaze
+            g = RobotGaze()
+            g.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE
+            g.pan = angles[0]
+            g.tilt = angles[1]
+            print ">> Sending Gaze Type:", g
+            self.rc.robot_controller.set_gaze_target(g, False)
+
+    def run_subscriber(self):
+        print(">>> Initializing Gaze Subscriber")
+        person_subscriber = rospy.Subscriber(self.inscope, People, self.people_callback, queue_size=1)
+        while self.run:
+            time.sleep(1)
+        person_subscriber.unregister()
+        print ">>> Deactivating ROS Subscriber"
+
+    def derive_gaze_angle(self):
+        pass
+
+
+class AffineTransform:
+    """
+    Derives the transformation between screen
+    coordinates in pixels and joint axis angles in degree.
+    """
+    def __init__(self):
+        print(">>> Initializing Affine Transform")
+        # Target ---> The ones you want to map to
+        self.target0 = [1.0, 1.0]
+        self.target1 = [1.0, 1.0]
+        self.target2 = [1.0, 1.0]
+        self.target3 = [1.0, 1.0]
+
+        # Origin ---> The ones that are mapped to [target0, target1, target2, target3]
+        self.origin0 = [1.0, 1.0]
+        self.origin1 = [1.0, 1.0]
+        self.origin2 = [1.0, 1.0]
+        self.origin3 = [1.0, 1.0]
+
+        # Divider
+        self.divider = 1.0
+
+        # Calculated and mapped Coordinates
+        mappedCoords = [1.0, 1.0]
+
+        # Affine transformation coefficients
+        self.An = 1.0
+        self.Bn = 1.0
+        self.Cn = 1.0
+        self.Dn = 1.0
+        self.En = 1.0
+        self.Fn = 1.0
+
+        # Test coord
+        self.test = [1.0, 1.0]
+
+    def set_coords(self):
+
+        # This is the target coordinate system
+        # Upper left corner
+        self.target0[0] = -45.0
+        self.target0[1] = 45.0
+
+        # Lower left corner
+        self.target1[0] = -45.0
+        self.target1[1] = -45.0
+
+        # Upper right corner
+        self.target2[0] = 45.0
+        self.target2[1] = 45.0
+
+        # Lower right corner
+        self.target3[0] = 45.0
+        self.target3[1] = -45.0
+
+        # This is the origin system, is mapped to [t0,t1,t2,t3]
+        # Upper left corner
+        self.origin0[0] = 0.0
+        self.origin0[1] = 0.0
+
+        # Lower left corner
+        self.origin1[0] = 0.0
+        self.origin1[1] = 240.0
+
+         # Upper right corner
+        self.origin2[0] = 320.0
+        self.origin2[1] = 0.0
+
+         # Lower right corner
+        self.origin3[0] = 320.0
+        self.origin3[1] = 240.0
+
+        # And finally the test coordinate
+        self.test[0] = 512.0
+        self.test[1] = 384.0
+
+    def calculate_divider(self):
+        result = ((self.origin0[0] - self.origin2[0]) * (self.origin1[1] - self.origin2[1])) - \
+                 ((self.origin1[0] - self.origin2[0]) * (self.origin0[1] - self.origin2[1]))
+
+        if result == 0.0:
+            print(">> Divider is ZERO - Check your Coordinates?")
+            sys.exit(1)
+        else:
+            self.divider = result
+            print(">> Divider " + str(self.divider))
+            self.calculateAn()
+            self.calculateBn()
+            self.calculateCn()
+            self.calculateDn()
+            self.calculateEn()
+            self.calculateFn()
+
+        return result
+
+    def calculateAn(self):
+        result = ((self.target0[0] - self.target2[0]) * (self.origin1[1] - self.origin2[1])) - \
+                 ((self.target1[0] - self.target2[0]) * (self.origin0[1] - self.origin2[1]))
+        self.An = result
+        print(">> An " + str(self.An))
+        return result
+
+    def calculateBn(self):
+        result = ((self.origin0[0] - self.origin2[0]) * (self.target1[0] - self.target2[0])) - \
+                 ((self.target0[0] - self.target2[0]) * (self.origin1[0] - self.origin2[0]))
+        self.Bn = result
+        print(">> Bn " + str(self.Bn))
+        return result
+
+    def calculateCn(self):
+        result = (self.origin2[0] * self.target1[0] - self.origin1[0] * self.target2[0]) * self.origin0[1] + \
+                 (self.origin0[0] * self.target2[0] - self.origin2[0] * self.target0[0]) * self.origin1[1] + \
+                 (self.origin1[0] * self.target0[0] - self.origin0[0] * self.target1[0]) * self.origin2[1]
+        self.Cn = result
+        print(">> Cn " + str(self.Cn))
+        return result
+
+    def calculateDn(self):
+        result = ((self.target0[1] - self.target2[1]) * (self.origin1[1] - self.origin2[1])) - \
+                 ((self.target1[1] - self.target2[1]) * (self.origin0[1] - self.origin2[1]))
+        self.Dn = result
+        print(">> Dn " + str(self.Dn))
+        return result
+
+    def calculateEn(self):
+        result = ((self.origin0[0] - self.origin2[0]) * (self.target1[1] - self.target2[1])) - \
+                 ((self.target0[1] - self.target2[1]) * (self.origin1[0] - self.origin2[0]))
+        self.En = result
+        print(">> En " + str(self.En))
+        return result
+
+    def calculateFn(self):
+        result = (self.origin2[0] * self.target1[1] - self.origin1[0] * self.target2[1]) * self.origin0[1] + \
+                 (self.origin0[0] * self.target2[1] - self.origin2[0] * self.target0[1]) * self.origin1[1] + \
+                 (self.origin1[0] * self.target0[1] - self.origin0[0] * self.target1[1]) * self.origin2[1]
+        self.Fn = result
+        print(">> Fn " + str(self.Fn))
+        return result
+
+    def derive_mapping_coords(self, point):
+        # r->x = ((matrixPtr->An * ad->x) + (matrixPtr->Bn * ad->y) + matrixPtr->Cn) / matrixPtr->Divider
+        # r->y = ((matrixPtr->Dn * ad->x) + (matrixPtr->En * ad->y) + matrixPtr->Fn) / matrixPtr->Divider
+        if self.divider != 0.0:
+            x = ((self.An * point[0]) + (self.Bn * point[1]) + self.Cn) / self.divider
+            y = ((self.Dn * point[0]) + (self.En * point[1]) + self.Fn) / self.divider
+            result = [x, y]
+            # print ">> Current Coordinate (Face):", point
+            print ">> x-pixels were mapped to angle: %s \n>> y-pixels were mapped to angle: %s" % (str(x), str(y))
+            return result
+        else:
+            return None
+
+
+def runner(arguments):
+    if len(arguments) != 3:
+        print(">>> Usage: simple_robot_gaze.py <inscope 'persons_scope'> <outscope 'gaze_target_scope'>\n\n")
+        sys.exit(1)
+
+    rd = RobotDriver("ROS", sys.argv[2])
+    at = AffineTransform()
+    at.set_coords()
+    at.calculate_divider()
+    gc = GazeController(rd, at, sys.argv[1])
+    gc.run_subscriber()
+
+if __name__ == '__main__':
+    runner(sys.argv)
+
diff --git a/client/python/setup.py b/client/python/setup.py
index 30b012b..cc58786 100644
--- a/client/python/setup.py
+++ b/client/python/setup.py
@@ -108,7 +108,7 @@ setup(
     entry_points={
         'console_scripts': [
             'hlrc_test_gui=hlrc_client.hlrc_test_gui:main',
-            'simple_robot_gaze=hlrc_client.simple-robot-gaze:main'
+            'simple_robot_gaze=hlrc_client.simple_robot_gaze:main'
         ],
     },
 )
