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 rospy

import actionlib

from hlrc_server.msg import *

from actionlib_msgs.msg import GoalStatus

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, timeout=None):

        """initialise

        :param scope: base scope we want to listen on

        """

        #init base settings

        Middleware.__init__(self,scope,loglevel)

        #ros-specific initialization

        if timeout is None: timeout = rospy.Duration()

        self.init_middleware(timeout)

    def __del__(self):

        """destructor

        """

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

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

    def init_middleware(self, timeout):

        """initialise middleware

        """

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

        try:

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

        except rospy.exceptions.ROSException as e:

            self.logger.info(str(e))

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

        start = time.time()

        def create_client(sub_scope, action, name):

            if not timeout.is_zero():

                t = rospy.Duration.from_sec(timeout.to_sec() - (time.time() - start)) # time left

                if t <= rospy.Duration():

                    raise Exception("timeout while connecting to actionlib servers")

            else:

                t = timeout

            self.logger.debug("creating %s client" % name)

            client = actionlib.SimpleActionClient(self.base_scope + sub_scope, action)

            if not client.wait_for_server(t):

                raise Exception("timeout while creating %s client" % name)

            return client

        self.speech_client = create_client("/set/speech", speechAction, "speech action")

        self.default_emotion_client = create_client("/set/defaultEmotion", emotionstateAction, "default emotion")

        self.current_emotion_client = create_client("/set/currentEmotion", emotionstateAction, "current emotion")

        self.animation_client = create_client("/set/animation", animationAction, "animation")

        self.gazetarget_client = create_client("/set/gaze", gazetargetAction, "gazetarget")

        self.lookattarget_client = create_client("/set/lookat", lookattargetAction, "lookattarget")

        self.mouthtarget_client = create_client("/set/mouth", mouthtargetAction, "mouthtarget")

        self.logger.info("MiddlewareROS initialised")

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

    def is_running(self):

        return not rospy.is_shutdown()

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

    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

        if (blocking):

            #send and wait:

            state = client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

            if (state != GoalStatus.SUCCEEDED):

                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))

        else:

            #non blocking, fire & forget

            client.send_goal(goal)

        self.logger.debug("emotion 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_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

        if (blocking):

            #send and wait:

            state = self.animation_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

            if (state != GoalStatus.SUCCEEDED):

                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))

        else:

            #non blocking, fire & forget

            self.animation_client.send_goal(goal)

        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.gaze_timestamp = rospy.Time.from_sec(gaze.gaze_timestamp.to_seconds())

        #send the goal to the server

        if (blocking):

            #send and wait:

            state = self.gazetarget_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

            if (state != GoalStatus.SUCCEEDED):

                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))

        else:

            #non blocking, fire & forget

            self.gazetarget_client.send_goal(goal)

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

    def publish_lookat_target(self, x, y, z, blocking, frame_id='', roll=0.0):

        """publish a gaze target via mw

        :param x,y,z: position to look at (in eyes frame or frame_id)

        :param roll: side-ways motion of head

        :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 = lookattargetGoal()

        p = goal.point

        p.header.frame_id = frame_id

        p.header.stamp = rospy.Time.now()

        p.point.x = float(x)

        p.point.y = float(y)

        p.point.z = float(z)

        goal.roll = roll

        # send the goal to the server

        if blocking:

            # send and wait:

            state = self.lookattarget_client.send_goal_and_wait(goal, execute_timeout=rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

            if state != GoalStatus.SUCCEEDED:

                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))

        else:

            # non blocking, fire & forget

            self.lookattarget_client.send_goal(goal)

        self.logger.debug("lookat 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

        if (blocking):

            #send and wait:

            state = self.mouthtarget_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

            if (state != GoalStatus.SUCCEEDED):

                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))

        else:

            #non blocking, fire & forget

            self.mouthtarget_client.send_goal(goal)

        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

        if (blocking):

            #send and wait:

            state = self.speech_client.send_goal_and_wait(goal, execute_timeout = rospy.Duration.from_sec(MiddlewareROS.ROS_ACTION_CALL_TIMEOUT))

            if (state != GoalStatus.SUCCEEDED):

                self.logger.error("%s timed out waiting for result or soemthing else went wrong" % (sys._getframe().f_code.co_name))

        else:

            #non blocking, fire & forget

            self.speech_client.send_goal(goal)

        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