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 roslib; 

import rospy 

import actionlib

from hlrc_server.msg import *

class MiddlewareROS(Middleware):

        #default timeout to wait in case of blocking calls

        ROS_ACTION_CALL_TIMEOUT = 30.0

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

        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

                """

                self.logger.info("initialising ROS middleware connection on scope %s" % (self.base_scope))

                rospy.init_node('hlrc_client', anonymous=True)

                self.logger.info("creating action clients")

                self.logger.debug("creating speech action client")

                self.speech_client = actionlib.SimpleActionClient(self.base_scope + "/set/speech", speechAction)

                self.speech_client.wait_for_server()

                self.logger.debug("creating default emotion action client")

                self.default_emotion_client = actionlib.SimpleActionClient(self.base_scope + "/set/defaultEmotion", emotionstateAction)

                self.default_emotion_client.wait_for_server()

                self.logger.debug("creating current emotion action client")

                self.current_emotion_client = actionlib.SimpleActionClient(self.base_scope + "/set/currentEmotion", emotionstateAction)

                self.current_emotion_client.wait_for_server()

                self.logger.debug("creating animation action client")

                self.animation_client = actionlib.SimpleActionClient(self.base_scope + "/set/animation", animationAction)

                self.animation_client.wait_for_server()

                self.logger.debug("creating gazetarget action client")

                self.gazetarget_client = actionlib.SimpleActionClient(self.base_scope + "/set/gaze", gazetargetAction)

                self.gazetarget_client.wait_for_server()

                self.logger.debug("creating mouthtarget action client")

                self.mouthtarget_client = actionlib.SimpleActionClient(self.base_scope + "/set/mouth", mouthtargetAction)

                self.mouthtarget_client.wait_for_server()

                self.logger.info("MiddlewareROS initialised")

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

        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

                """

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

                #create a goal to send to the action server.

                goal = emotionstateGoal()

                goal.value    = self.convert_emotiontype_to_rosval(emotion.value)

                goal.duration =  int(emotion.time_ms)

                #which client do we use?

                if (em_type == RobotEmotion.TYPE_DEFAULT):

                        client = self.default_emotion_client

                else:

                        client = self.current_emotion_client

                #send the goal to the server

                client.send_goal(goal)

                #wait for the server to finish 

                if (blocking):

                        timed_out = client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

                        if (timed_out):

                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))

        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_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 a goal to send to the action server.

                goal = animationGoal()

                goal.target = self.convert_animationtype_to_rosval(animation.value)

                goal.repetitions = animation.repetitions

                goal.duration_each =  int(animation.time_ms)

                goal.scale = animation.scale

                #send the goal to the server

                self.animation_client.send_goal(goal)

                #wait for the server to finish 

                if (blocking):

                        timed_out = self.animation_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

                        if (timed_out):

                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))

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

        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 a goal to send to the action server.

                goal = gazetargetGoal()

                goal.pan  = gaze.pan

                goal.tilt = gaze.tilt

                goal.roll = gaze.roll

                goal.vergence = gaze.vergence

                goal.pan_offset  = gaze.pan_offset

                goal.tilt_offset = gaze.tilt_offset

                #type

                if (gaze.gaze_type == RobotGaze.GAZETARGET_ABSOLUTE):

                        goal.gaze_type = gazetargetGoal.GAZETARGET_ABSOLUTE

                else:

                        goal.gaze_type = gazetargetGoal.GAZETARGET_RELATIVE

                goal.timestamp = rospy.Time.from_sec(gaze.timestamp)

                #send the goal to the server

                self.gazetarget_client.send_goal(goal)

                #wait for the server to finish 

                if (blocking):

                        timed_out = self.gazetarget_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

                        if (timed_out):

                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))

                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 a goal to send to the action server.

                goal = mouthtargetGoal()

                goal.opening_left  = mouth.opening_left

                goal.opening_center = mouth.opening_center

                goal.opening_right = mouth.opening_right

                goal.position_left = mouth.position_left

                goal.position_center  = mouth.position_center

                goal.position_right = mouth.position_right

                #send the goal to the server

                self.mouthtarget_client.send_goal(goal)

                #wait for the server to finish 

                if (blocking):

                        timed_out = self.mouthtarget_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

                        if (timed_out):

                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))

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

                #create a goal to send to the action server.

                goal = speechGoal(text=text_)

                #send the goal to the server

                self.speech_client.send_goal(goal)

                #wait for the server to finish 

                if (blocking):

                        timed_out = self.speech_client.wait_for_result(rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

                        if (timed_out):

                                self.logger.error("%s timed out waiting for result" % (sys._getframe().f_code.co_name))

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

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

        # some helpers

        def convert_animationtype_to_rosval(self, value):

                """convert RobotAnimation.value to ROS 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 animationGoal.IDLE

                elif (value == RobotAnimation.HEAD_NOD):

                        return animationGoal.HEAD_NOD

                elif (value == RobotAnimation.HEAD_SHAKE):

                        return animationGoal.HEAD_SHAKE

                elif (value == RobotAnimation.EYEBLINK_L):

                        return animationGoal.EYEBLINK_L

                elif (value == RobotAnimation.EYEBLINK_R):

                        return  animationGoal.EYEBLINK_R

                elif (value == RobotAnimation.EYEBLINK_BOTH):

                        return  animationGoal.EYEBLINK_BOTH

                elif (value == RobotAnimation.EYEBROWS_RAISE):

                        return  animationGoal.EYEBROWS_RAISE

                elif (value == RobotAnimation.EYEBROWS_LOWER):

                        return  animationGoal.EYEBROWS_LOWER

                else:

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

                        return  animationGoal.NEUTRAL

        def convert_emotiontype_to_rosval(self, value):

                """convert RobotEmotion.value to ROS animation value

                :param value: RobotEmotion.* id to convert to ros 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 emotionstateGoal.NEUTRAL

                elif (value == RobotEmotion.HAPPY):

                        return emotionstateGoal.HAPPY

                elif (value == RobotEmotion.SAD):

                        return emotionstateGoal.SAD

                elif (value == RobotEmotion.ANGRY):

                        return emotionstateGoal.ANGRY

                elif (value == RobotEmotion.SURPRISED):

                        return  emotionstateGoal.SURPRISED

                elif (value == RobotEmotion.FEAR):

                        return emotionstateGoal.FEAR

                else:

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

                        return  emotionstateGoal.NEUTRAL