hlrc

"""

This file is part of hlrc

Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

http://opensource.cit-ec.de/projects/hlrc

This file may be licensed under the terms of the

GNU General Public License Version 3 (the ``GPL''),

or (at your option) any later version.

Software distributed under the License is distributed

on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

express or implied. See the GPL for the specific language

governing rights and limitations.

You should have received a copy of the GPL along with this

program. If not, go to http://www.gnu.org/licenses/gpl.html

or write to the Free Software Foundation, Inc.,

51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

The development of this software was supported by the

Excellence Cluster EXC 277 Cognitive Interaction Technology.

The Excellence Cluster EXC 277 is a grant of the Deutsche

Forschungsgemeinschaft (DFG) in the context of the German

Excellence Initiative.

"""

from Middleware import *

import errno

import rsb

import rsb.converter

import rst

from rstsandbox.animation.EmotionExpression_pb2 import EmotionExpression

from rstsandbox.animation.HeadAnimation_pb2 import HeadAnimation

from rstsandbox.animation.BinocularHeadGaze_pb2 import BinocularHeadGaze

from rstsandbox.geometry.SphericalDirectionFloat_pb2 import SphericalDirectionFloat

class MiddlewareRSB(Middleware):

    #######################################################################

    def __init__(self, scope, loglevel=logging.WARNING):

        """initialise

        :param scope: base scope we want to listen on

        """

        #init base settings

        Middleware.__init__(self,scope,loglevel)

        #call mw init

        self.init_middleware()

    def __del__(self):

        """destructor

        """

        self.logger.debug("destructor of MiddlewareROS called")

    #######################################################################

    def init_middleware(self):

        """initialise middleware

        """

        #mute rsb logging:

        logging.getLogger("rsb").setLevel(logging.ERROR)

        #initialise RSB stuff

        self.logger.info("initialising RSB middleware connection on scope %s, registering rst converters..." % (self.base_scope))

        self.EmotionExpression_converter = rsb.converter.ProtocolBufferConverter(messageClass = EmotionExpression)

        rsb.converter.registerGlobalConverter(self.EmotionExpression_converter)

        self.HeadAnimation_converter = rsb.converter.ProtocolBufferConverter(messageClass = HeadAnimation)

        rsb.converter.registerGlobalConverter(self.HeadAnimation_converter)

        self.BinocularHeadGaze_converter = rsb.converter.ProtocolBufferConverter(messageClass = BinocularHeadGaze)

        rsb.converter.registerGlobalConverter(self.BinocularHeadGaze_converter)

        try:

            self.server = rsb.createRemoteServer(self.base_scope + '/set')

        except ValueError:

            self.logger.error("ERROR: invalid scope given. server deactivated")

            self.server.deactivate()

            sys.exit(errno.EINVAL)

    #######################################################################

    def publish_emotion(self, em_type, emotion, blocking):

        """publish an emotion via mw

        :param em_type: type of emotion (RobotEmotion::TYPE_DEFAULT or RobotEmotion::TYPE_CURRENT)

        :param emotion: emotion to set

        :param blocking: True if this call should block until execution finished on robot

        """

        #create emotion & fill it with values:

        rsb_em = EmotionExpression()

        #set value

        rsb_em.value = self.convert_emotiontype_to_rsbval(emotion.value)

        #set duration

        rsb_em.duration = int(emotion.time_ms)

        with rsb.createRemoteServer(self.base_scope + '/set') as server:

            self.logger.debug("calling the emotion rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))

            if (blocking):

                #blocking rpc call:

                if (em_type == EmotionExpression.TYPE_DEFAULT):

                    result = server.defaultEmotion(rsb_em)

                else:

                    result = server.currentEmotion(rsb_em)

                self.logger.debug("server reply: '%s'" % result)

            else:

                if (em_type == EmotionExpression.TYPE_DEFAULT):

                    future = server.defaultEmotion.async(rsb_em)

                else:

                    future = server.currentEmotion.async(rsb_em)

                #we could block here for a incoming result with a timeout in s

                #print '> server reply: "%s"' % future.get(timeout = 10);

            self.logger.debug("emotion rpc done")

    def publish_head_HeadAnimation(self, HeadAnimation, blocking):

        """publish an head HeadAnimation via mw

        :param HeadAnimation: HeadAnimation to set

        :param blocking: True if this call should block until execution finished on robot

        """

        self.logger.debug("calling the HeadAnimation rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))

        #create HeadAnimation & fill it with values:

        rsb_ani = HeadAnimation()

        #select ani

        rsb_ani.target = self.convert_HeadAnimationtype_to_rsbval(HeadAnimation.value)

        rsb_ani.repetitions = HeadAnimation.repetitions

        rsb_ani.duration_each = HeadAnimation.time_ms

        rsb_ani.scale       = HeadAnimation.scale

        if (blocking):

            #blocking:

            result = self.server.HeadAnimation(rsb_ani)

            self.logger.debug("server reply: '%s'" % result)

        else:

            future = self.server.HeadAnimation.async(rsb_ani)

            #we can block here for a incoming result with a timeout in s

            #print '> server reply: "%s"' % future.get(timeout = 10);

        self.logger.debug("HeadAnimation rpc done")

    def publish_default_emotion(self, emotion, blocking):

        self.publish_emotion(RobotEmotion.TYPE_DEFAULT, emotion, blocking)

    def publish_current_emotion(self, emotion, blocking):

        self.publish_emotion(RobotEmotion.TYPE_CURRENT, emotion, blocking)

    def publish_gaze_target(self, gaze, blocking):

        """publish a gaze target via mw

        :param gaze: gaze to set

        :param blocking: True if this call should block until execution finished on robot

        """

        self.logger.debug("calling the gaze rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))

        #create gaze target & fill it with values:

        rsb_gaze = BinocularHeadGaze()

        #fill proto

            rsb_gaze.target = SphericalDirectionFloat()

            rsb_gaze.target.elevation = gaze.tilt

            rsb_gaze.target.azimuth   = gaze.pan

        rsb_gaze.vergence = gaze.vergence

            rsb_gaze.offset = SphericalDirectionFloat()

            rsb_gaze.offset.elevation = gaze.tilt_offset

            rsb_gaze.offset.azimuth   = gaze.pan_offset

        if (blocking):

            #blocking:

            result = self.server.gaze(rsb_gaze)

            self.logger.debug("server reply: '%s'" % result)

        else:

            future = self.server.gaze.async(rsb_gaze)

            #we can block here for a incoming result with a timeout in s

            #print '> server reply: "%s"' % future.get(timeout = 10);

        self.logger.debug("gaze rpc done")

    """

    def publish_mouth_target(self, mouth, blocking):

        publish a mouth target via mw

        :param mouth: mouth value to set

        :param blocking: True if this call should block until execution finished on robot

        self.logger.debug("calling the mouth rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))

        #create mouth state & fill it with values:

        rsb_mouth = MouthTarget()

        #fill proto

        rsb_mouth.opening_left   = mouth.opening_left

        rsb_mouth.opening_center = mouth.opening_center

        rsb_mouth.opening_right  = mouth.opening_right

        rsb_mouth.position_left  = mouth.position_left

        rsb_mouth.position_center = mouth.position_center

        rsb_mouth.position_right = mouth.position_right

        if (blocking):

            #blocking:

            result = self.server.mouth(rsb_mouth)

            self.logger.debug("server reply: '%s'" % result)

        else:

            future = self.server.mouth.async(rsb_mouth)

            #we can block here for a incoming result with a timeout in s

            #print '> server reply: "%s"' % future.get(timeout = 10);

        self.logger.debug("mouth rpc done")

    """

    def publish_speech(self, text, blocking):

        """publish a tts request via mw

        :param text: text to synthesize and speak

        :param blocking: True if this call should block until execution finished on robot

        """

        self.logger.debug("calling the speech rpc (%s)..." % ("BLOCKING" if blocking else "NON-BLOCKING"))

        if (blocking):

            #blocking:

            result = self.server.speech(text)

            self.logger.debug("server reply: '%s'" % result)

        else:

            future = self.server.speech.async(text)

            #we can block here for a incoming result with a timeout in s

            #print '> server reply: "%s"' % future.get(timeout = 10);

        self.logger.debug("speech rpc done")

    #######################################################################

    def is_running(self):

        return True

    #######################################################################

    # some helpers

    def convert_HeadAnimationtype_to_rsbval(self, value):

        """convert RobotHeadAnimation.value to RSB HeadAnimation value

        :param value: RobotHeadAnimation.* id to convert to rsb id

        """

        #NOTE: this convertion is important as the actual integer values of

        #      thy python api and the protobuf might be different

        if (value == RobotHeadAnimation.IDLE):

            return HeadAnimation().IDLE

        elif (value == RobotHeadAnimation.HEAD_NOD):

            return HeadAnimation().HEAD_NOD

        elif (value == RobotHeadAnimation.HEAD_SHAKE):

            return HeadAnimation().HEAD_SHAKE

        elif (value == RobotHeadAnimation.EYEBLINK_L):

            return HeadAnimation().EYEBLINK_L

        elif (value == RobotHeadAnimation.EYEBLINK_R):

            return  HeadAnimation().EYEBLINK_R

        elif (value == RobotHeadAnimation.EYEBLINK_BOTH):

            return  HeadAnimation().EYEBLINK_BOTH

        elif (value == RobotHeadAnimation.EYEBROWS_RAISE):

            return  HeadAnimation().EYEBROWS_RAISE

        elif (value == RobotHeadAnimation.EYEBROWS_LOWER):

            return  HeadAnimation().EYEBROWS_LOWER

        else:

            self.logger.error("invalid HeadAnimation type %d\n" % (value))

            return  HeadAnimation().NEUTRAL

    def convert_emotiontype_to_rsbval(self, value):

        """convert RobotEmotion.value to RSB HeadAnimation value

        :param value: RobotEmotion.* id to convert to rsb id

        """

        #NOTE: this convertion is important as the actual integer values of

        #      thy python api and the protobuf might be different

        if (value == RobotEmotion.NEUTRAL):

            return EmotionExpression().NEUTRAL

        elif (value == RobotEmotion.HAPPY):

            return EmotionExpression().HAPPY

        elif (value == RobotEmotion.SAD):

            return EmotionExpression().SAD

        elif (value == RobotEmotion.ANGRY):

            return EmotionExpression().ANGRY

        elif (value == RobotEmotion.SURPRISED):

            return  EmotionExpression().SURPRISED

        elif (value == RobotEmotion.FEAR):

            return EmotionExpression().FEAR

        else:

            self.logger.error("invalid emotion type %d\n" % (value))

            return  EmotionExpression().NEUTRAL