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

try:

    import rst

    import rstsandbox

    from rst.robot.EmotionState_pb2 import EmotionState

    from rst.robot.Animation_pb2    import Animation

    from rst.robot.GazeTarget_pb2   import GazeTarget

    from rst.robot.MouthTarget_pb2  import MouthTarget

except ImportError, e:

    print("Warning, RSB could be imported, but the required message types do not exist: ")

    print e

#except ImportError as exception:

#    sys.stderr.write("ImportError: {}\n> HINT: try to export PYTHONPATH=$PYTHONPATH:$YOUR_PREFIX/lib/python2.7/site-packages/\n\n".format(exception))

#    sys.exit(errno.ENOPKG)

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.emotionstate_converter = rsb.converter.ProtocolBufferConverter(messageClass = EmotionState)

                rsb.converter.registerGlobalConverter(self.emotionstate_converter)

                self.animation_converter = rsb.converter.ProtocolBufferConverter(messageClass = Animation)

                rsb.converter.registerGlobalConverter(self.animation_converter)

                self.gaze_converter = rsb.converter.ProtocolBufferConverter(messageClass = GazeTarget)

                rsb.converter.registerGlobalConverter(self.gaze_converter)

                self.mouth_converter = rsb.converter.ProtocolBufferConverter(messageClass = MouthTarget)

                rsb.converter.registerGlobalConverter(self.mouth_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 = EmotionState()

                #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 == RobotEmotion.TYPE_DEFAULT):

                                        result = server.defaultEmotion(rsb_em)

                                else:

                                        result = server.currentEmotion(rsb_em)

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

                        else:

                                if (em_type == RobotEmotion.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_animation(self, animation, blocking):

                """publish an head animation via mw

                :param animation: animation to set

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

                """

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

                #create animation & fill it with values:

                rsb_ani = Animation()

                #select ani

                rsb_ani.target = self.convert_animationtype_to_rsbval(animation.value)

                rsb_ani.repetitions = animation.repetitions

                rsb_ani.duration_each = animation.time_ms

                rsb_ani.scale       = animation.scale

                if (blocking):

                        #blocking:

                        result = self.server.animation(rsb_ani)

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

                else:

                        future = self.server.animation.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("animation 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 = GazeTarget()

                #fill proto

                rsb_gaze.pan  = gaze.pan

                rsb_gaze.tilt = gaze.tilt

                rsb_gaze.roll = gaze.roll

                rsb_gaze.vergence = gaze.vergence

                rsb_gaze.pan_offset  = gaze.pan_offset

                rsb_gaze.tilt_offset = gaze.tilt_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")

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

        # some helpers

        def convert_animationtype_to_rsbval(self, value):

                """convert RobotAnimation.value to RSB animation value

                :param value: RobotAnimation.* 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 == RobotAnimation.IDLE):

                        return Animation().IDLE

                elif (value == RobotAnimation.HEAD_NOD):

                        return Animation().HEAD_NOD

                elif (value == RobotAnimation.HEAD_SHAKE):

                        return Animation().HEAD_SHAKE

                elif (value == RobotAnimation.EYEBLINK_L):

                        return Animation().EYEBLINK_L

                elif (value == RobotAnimation.EYEBLINK_R):

                        return  Animation().EYEBLINK_R

                elif (value == RobotAnimation.EYEBLINK_BOTH):

                        return  Animation().EYEBLINK_BOTH

                elif (value == RobotAnimation.EYEBROWS_RAISE):

                        return  Animation().EYEBROWS_RAISE

                elif (value == RobotAnimation.EYEBROWS_LOWER):

                        return  Animation().EYEBROWS_LOWER

                else:

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

                        return  Animation().NEUTRAL

        def convert_emotiontype_to_rsbval(self, value):

                """convert RobotEmotion.value to RSB animation 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 EmotionState().NEUTRAL

                elif (value == RobotEmotion.HAPPY):

                        return EmotionState().HAPPY

                elif (value == RobotEmotion.SAD):

                        return EmotionState().SAD

                elif (value == RobotEmotion.ANGRY):

                        return EmotionState().ANGRY

                elif (value == RobotEmotion.SURPRISED):

                        return  EmotionState().SURPRISED

                elif (value == RobotEmotion.FEAR):

                        return EmotionState().FEAR

                else:

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

                        return  EmotionState().NEUTRAL