hlrc

__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)