コード例 #1
0
ファイル: robot.py プロジェクト: jdddog/hri
class Robot(Entity):
    ENTITY_TYPE = 'robot'

    def __init__(self, expression_enum, gesture_enum):
        Entity.__init__(self, Robot.ENTITY_TYPE, Robot.ENTITY_TYPE + str(1), None)
        self.expression_enum = expression_enum
        self.gesture_enum = gesture_enum

        self.gaze_client = actionlib.SimpleActionClient('gaze', TargetAction)
        self.gaze_found = False

        self.expression_client = MultiGoalActionClient('expression', ExpressionAction)
        self.expression_found = False

        self.tts_client = actionlib.SimpleActionClient('text_to_speech', TextToSpeechAction)
        self.tts_found = False

        self.gesture_client = MultiGoalActionClient('gesture', GestureAction)
        self.gesture_found = False

        self.tts_duration_srv = rospy.ServiceProxy('tts_subsentence_duration', TextToSpeechSubsentenceDuration)
        self.tts_duration_found = False

        self.event = None
        self.action_handles = []
        self.say_to_plan = SayToPlan(self)
        self.gaze_ah = None
        self.say_ah = None

        self.lock = threading.RLock()
        self.listen_cb = None
        self.listen_sub = None

    def register_listen_callback(self, callback):
        self.listen_cb = callback
        self.listen_sub = rospy.Subscriber("itf_listen", String, self.__listen, queue_size=10)

    def __listen(self, msg):

        if msg.data == 'BADINPUT':
            text = None
        else:
            text = msg.data

        self.listen_cb(text)

    # Text to speech
    def say(self, text):
        if not self.tts_found:
            self.wait_for_action_servers(self.tts_client)
            self.tts_found = True

        ParamFormatting.assert_types(self.say, text, str)
        goal = TextToSpeechGoal()
        goal.sentence = text
        self.tts_client.send_goal(goal, feedback_cb=self.say_feedback, done_cb=self.say_done)
        ah = SingleGoalActionHandle(self.tts_client)
        self.say_ah = ah
        self.add_action_handle(ah)
        return ah

    def say_and_wait(self, text):
        self.say(text)
        self.tts_client.wait_for_result()

    def say_done(self, state, result):
        with self.lock:
            self.remove_action_handle(self.say_ah)
            self.say_ah = None

    # Gaze
    def gaze(self, target, speed=0.5):
        if not self.gaze_found:
            self.wait_for_action_servers(self.gaze_client)
            self.gaze_found = True

        ParamFormatting.assert_types(self.gaze, target, Entity)
        ParamFormatting.assert_types(self.gaze, speed, float)
        ParamFormatting.assert_range(self.gaze, speed, 0.0, 1.0)

        World().add_to_world(target)
        goal = TargetGoal()
        goal.target = target.get_id()
        goal.speed = speed
        goal.acceleration = 0.3

        self.gaze_client.send_goal(goal, feedback_cb=self.gaze_feedback, done_cb=self.gaze_done)
        ah = SingleGoalActionHandle(self.gaze_client)
        self.gaze_ah = ah
        self.add_action_handle(ah)
        return ah

    def gaze_and_wait(self, target, speed=0.5, timeout=rospy.Duration()):
        self.gaze(target, speed)
        self.gaze_client.wait_for_result(timeout)

    def gaze_feedback(self, feedback):
        pass

    def gaze_done(self, state, result):
        with self.lock:
            self.remove_action_handle(self.gaze_ah)
            self.gaze_ah = None

    # Blinking
    def blink(self, blink_duration, blink_rate_mean, blink_rate_sd):
        """
        :param blink_duration:
        :param blink_rate_mean:
        :param blink_rate_sd:
        :return:
        """
        raise NotImplementedError("Please implement the blink method")

    # Facial expressions
    def expression(self, expression, intensity=None, speed=None, duration=None):
        if not self.expression_found:
            self.wait_for_action_servers(self.expression_client)
            self.expression_found = True

        ParamFormatting.assert_types(self.expression, expression, IExpression)

        goal = ExpressionGoal()
        goal.expression = expression.name

        if intensity is None:
            goal.intensity = -1
        else:
            ParamFormatting.assert_types(self.expression, intensity, float)
            ParamFormatting.assert_range(self.expression, intensity, 0.0, 1.0)
            goal.intensity = intensity

        if speed is None:
            goal.speed = -1
        else:
            ParamFormatting.assert_types(self.expression, speed, float)
            ParamFormatting.assert_range(self.expression, speed, 0.0, 1.0)
            goal.speed = speed

        if duration is None:
            goal.duration = -1
        else:
            ParamFormatting.assert_types(self.expression, duration, float)
            ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
            goal.duration = duration

        gh = self.expression_client.send_goal(goal, done_cb=self.expression_done)
        ah = MultiGoalActionHandle(self.expression_client, gh)
        self.add_action_handle(ah)
        return ah

    def expression_and_wait(self, expression, intensity=None, speed=None, duration=None, timeout=rospy.Duration()):
        ah = self.expression(expression, intensity, speed, duration)
        self.expression_client.wait_for_result(ah.goal_handle, timeout)

    def expression_done(self, goal_handle):
        with self.lock:
            ah = self.get_action_handle(goal_handle)
            self.remove_action_handle(ah)

    # Gestures
    def gesture(self, gesture, target=None, duration=None):
        if not self.gesture_found:
            self.wait_for_action_servers(self.gesture_client)
            self.gesture_found = True

        ParamFormatting.assert_types(self.gesture, gesture, IGesture)

        goal = GestureGoal()
        goal.gesture = gesture.name

        if target is None:
            goal.target = ''
        else:
            World().add_to_world(target)
            ParamFormatting.assert_types(self.gesture, target, Entity)
            goal.target = target.get_id()

        if duration is None:
            goal.duration = -1
        else:
            ParamFormatting.assert_types(self.expression, duration, float)
            ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
            goal.duration = duration

        gh = self.gesture_client.send_goal(goal, done_cb=self.gesture_done)

        ah = MultiGoalActionHandle(self.gesture_client, gh)
        self.add_action_handle(ah)
        return ah

    def gesture_and_wait(self, gesture, target=None, duration=None, timeout=rospy.Duration()):
        ah = self.gesture(gesture, target, duration)
        self.gesture_client.wait_for_result(ah.goal_handle, timeout)

    def gesture_done(self, goal_handle):
        with self.lock:
            ah = self.get_action_handle(goal_handle)
            self.remove_action_handle(ah)

    # Speaking, gazing and gesturing simultaneously
    def say_to(self, text, audience):
        if not self.tts_duration_found:
            self.wait_for_services(self.tts_duration_srv)
            self.tts_duration_found = True

        ParamFormatting.assert_types(self.say_to, text, str)
        ParamFormatting.assert_types(self.say_to, audience, Entity, Query, list)

        self.say_to_plan.parse_parameters(text, audience, self.expression_enum, self.gesture_enum, self.tts_duration_srv)

        ah = self.say_to_plan.execute()
        self.add_action_handle(ah)
        return ah

    def say_to_and_wait(self, text, audience):
        ah = self.say_to(text, audience)
        self.wait(ah)

    def say_feedback(self, feedback):
        if feedback.current_word_index in self.say_to_plan.gaze_change_locations:
            if isinstance(self.say_to_plan.audience, Entity):
                person = self.say_to_plan.audience
                self.say_to_plan.current_gazee = person
                self.say_to_plan.gaze_ah = self.gaze(person.head)

            elif isinstance(self.say_to_plan.audience, Query):
                people = self.say_to_plan.audience.execute()

                if len(people) > 1:
                    #if self.say_to_plan.current_gazee in people:
                    #    people.remove(self.say_to_plan.current_gazee)

                    person = random.choice(people)
                    self.say_to_plan.current_gazee = person
                    self.say_to_plan.gaze_ah = self.gaze(person.head)
                elif len(people) == 1:
                    person = people[0]
                    self.say_to_plan.current_gazee = person
                    self.say_to_plan.gaze_ah = self.gaze(person.head)

        if feedback.current_word_index in self.say_to_plan.expression_lookup:
            expression = self.say_to_plan.expression_lookup[feedback.current_word_num]
            ahs = self.do(expression)
            self.say_to_plan.add_action_handle(ahs[0])

        if feedback.current_word_index in self.say_to_plan.gesture_lookup:
            gesture = self.say_to_plan.gesture_lookup[feedback.current_word_index]
            ahs = self.do(gesture)
            self.say_to_plan.add_action_handle(ahs[0])

    def add_action_handle(self, action_handle):
        with self.lock:
            ParamFormatting.assert_types(self.add_action_handle, action_handle, IActionHandle)
            self.action_handles.append(action_handle)

    def get_action_handle(self, goal_handle):
        with self.lock:
            for ah in self.action_handles:
                if isinstance(ah, MultiGoalActionHandle):
                    if ah.goal_handle == goal_handle:
                        return ah

    def remove_action_handle(self, action_handle):
        with self.lock:
            ParamFormatting.assert_types(self.remove_action_handle, action_handle, IActionHandle)
            if action_handle in self.action_handles:
                    self.action_handles.remove(action_handle)

    def do(self, *goals):
        action_handles = []

        with self.lock:
            for goal in goals:
                if isinstance(goal, TextToSpeechGoal):
                    self.tts_client.send_goal(goal)
                    ah = SingleGoalActionHandle(self.tts_client)
                    self.add_action_handle(ah)
                    action_handles.append(ah)

                elif isinstance(goal, TargetGoal):
                    self.gaze_client.send_goal(goal)
                    ah = SingleGoalActionHandle(self.gaze_client)
                    self.add_action_handle(ah)
                    action_handles.append(ah)

                elif isinstance(goal, ExpressionGoal):
                    gh = self.expression_client.send_goal(goal)
                    ah = MultiGoalActionHandle(self.expression_client, gh)
                    self.add_action_handle(ah)
                    action_handles.append(ah)

                elif isinstance(goal, GestureGoal):
                    gh = self.gesture_client.send_goal(goal)
                    ah = MultiGoalActionHandle(self.gesture_client, gh)
                    self.add_action_handle(ah)
                    action_handles.append(ah)
                else:
                    raise TypeError('robot.do parameter goals has a goal with a unsupported type {0}, at index {1}. Should be one of: TextToSpeechGoal, TargetGoal, ExpressionGoal or GestureGoal'.format(type(goal), goals.index(goal)))

        return action_handles

    # Wait for one or more goals to finish
    def wait(self, *action_handles):
        for ah in action_handles:
            ParamFormatting.assert_types(self.wait, ah, IActionHandle)
            ah.wait_for_result()
            self.remove_action_handle(action_handle=ah)

    # Cancel one or more goals
    def cancel(self, *action_handles):
        for ah in action_handles:
            ParamFormatting.assert_types(self.wait, ah, IActionHandle)
            ah.cancel_action()
            self.remove_action_handle(action_handle=ah)

    def default_tf_frame_id(self):
        raise NotImplementedError("Please implement this method")

    def tf_frame_id(self):
        raise NotImplementedError("Please implement this method")

    # Wait for a period of time
    def wait_for_period(self, period):
        self.event = threading.Event()
        self.event.wait(timeout=period)

    def cancel_wait_for_period(self):
        if self.event is not None:
            self.event.set()
コード例 #2
0
ファイル: robot.py プロジェクト: linas/hri
class Robot(Entity):
    """ The Robot class represents a robot. It contains high level functions for representing and controlling robot
    actions, receiving callbacks about action events and subscribing to social communication.

    .. note::
        The Robot class is an abstract class and is never directly used to program a robot. Instead, each robot has
        its own class derived from Robot, e.g. the Nao robot is represented by the Nao class.

        See here for more details: http://en.wikipedia.org/wiki/Abstract_type

    """

    local_id = 0    # Id counter for robot classes
    SERVER_TIMEOUT = 5.0    # Timeout for connecting to action servers and services

    def __init__(self):
        self.local_id = Robot.local_id
        Robot.local_id += 1
        Entity.__init__(self, 'robot{0}'.format(self.local_id), None)

        robot_actions = Robot.read_supported_actions()
        robot_services = Robot.read_supported_services()

        self.say_found = False
        self.gaze_found = False
        self.gesture_found = False
        self.expression_found = False
        self.say_duration_found = False

        # Create the action and service proxies that connect to vendor specific action servers and services.
        if SayAction.__name__ in robot_actions:
            namespace = robot_actions[SayAction.__name__]['namespace']
            self.say_client = actionlib.SimpleActionClient(namespace, SayAction)
            self.say_found = Robot.wait_for_action_server(self.say_client, SayAction.__name__)
        else:
            self.say_client = None

        if GazeAction.__name__ in robot_actions:
            namespace = robot_actions[GazeAction.__name__]['namespace']
            self.gaze_client = actionlib.SimpleActionClient(namespace, TargetAction)
            self.gaze_found = Robot.wait_for_action_server(self.gaze_client, GazeAction.__name__)
        else:
            self.gaze_client = None

        if GestureAction.__name__ in robot_actions:
            namespace = robot_actions[GestureAction.__name__]['namespace']
            self.gesture_client = MultiGoalActionClient(namespace, GestureAction)
            self.gesture_found = Robot.wait_for_action_server(self.gesture_client, GestureAction.__name__)
        else:
            self.gesture_client = None

        if ExpressionAction.__name__ in robot_actions:
            namespace = robot_actions[Action.expression.name]['namespace']
            self.expression_client = MultiGoalActionClient(namespace, ExpressionAction)
            self.expression_found = Robot.wait_for_action_server(self.gesture_client, ExpressionAction.__name__)
        else:
            self.expression_client = None

        if SayDuration.__name__ in robot_services:
            name = robot_services[SayDuration.__name__]['name']
            self.say_duration_srv = rospy.ServiceProxy(namespace, SayDuration)
            self.say_duration_found = Robot.wait_for_service(self.say_duration_srv, SayDuration.__name__)
        else:
            self.say_duration_srv = None

        self.gaze_ah = None
        self.say_ah = None

        self.lock = threading.RLock()
        self.listen_cb = None
        self.listen_sub = None

    def default_body_part(self):
        raise NotImplementedError("Please implement this method")

    def register_listen_callback(self, callback):
        self.listen_cb = callback
        self.listen_sub = rospy.Subscriber("itf_listen", String, self.__listen, queue_size=10)

    def __listen(self, msg):

        if msg.data == 'BADINPUT':
            text = None
        else:
            text = msg.data

        self.listen_cb(text)

    def say(self, text, feedback_cb=None, done_cb=None):
        """ Say the text in the variable text.

        This function is asynchronous; it returns an ActionHandle, a unique identifier for the action being performed.
        An action handle can be used to wait for an action to complete or to stop an action

        :param text: The text the robot should say
        :type text: str

        :param feedback_cb: a function that will be called when the state of the say action changes. The function
        should have one parameter, which will be filled with a SayActionFeedback instance.
        :type feedback_cb: Callable

        :param done_cb: a function that will be called when the say action finishes.
        :type done_cb: Callable

        :return: an action handle to keep track of the action
        :rtype: SingleGoalActionHandle
        :raises TypeError: text is not a str, feedback_cb is not None or Callable, done_cb is not None or Callable
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (str, (None, Callable), (None, Callable)),
                            text, feedback_cb, done_cb)

        if Robot.is_action_runnable(self.say_client, SayAction.__name__, self.say_found):
            goal = SayGoal(text)
            self.say_client.send_goal(goal, feedback_cb, done_cb)
            ah = SingleGoalActionHandle(self.say_client)
            self.say_ah = ah
            return ah

    def say_and_wait(self, text, feedback_cb=None, done_cb=None):
        """ A synchronous version of the say function.

        The and_wait syntax signals that the function will not return until it completes.

        No ActionHandle is returned because the action completes before it returns.
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (str, (None, Callable), (None, Callable)),
                            text, feedback_cb, done_cb)

        self.say(text, feedback_cb, done_cb)
        self.say_client.wait_for_result()

    def gaze(self, target, speed=0.5, feedback_cb=None, done_cb=None):
        """ Makes the robot gaze at a target.

        This function is asynchronous and returns an SingleGoalActionHandle to keep track of the action.

        :param target: The target to gaze at
        :type target: Entity

        :param speed: The speed of the gaze action (normalised between 0.0 < speed <= 1.0)
        :type speed: float

        :param feedback_cb: a function that will be called when the state of the gaze action changes. The function
        should have one parameter, which will be filled with a GazeActionFeedback instance.
        :type feedback_cb: Callable

        :param done_cb: a function that will be called when the gaze action finishes.
        :type done_cb: Callable

        :return: an action handle to keep track of the action
        :rtype: SingleGoalActionHandle
        :raises TypeError: target is not an Entity, speed is not a float, feedback_cb is not None or Callable, done_cb
        is not None or Callable
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (Entity, float, (None, Callable), (None, Callable)),
                            target, speed, feedback_cb, done_cb)

        if Robot.is_action_runnable(self.gaze_client, GazeAction.__name__, self.gaze_found):
            World().add_to_world(target)
            goal = GazeGoal(target, speed, 0.3) #Todo override acceleration goal.acceleration = 0.3
            self.gaze_client.send_goal(goal, feedback_cb, done_cb)
            ah = SingleGoalActionHandle(self.gaze_client)
            self.gaze_ah = ah
            return ah

    def gaze_and_wait(self, target, speed=0.5, feedback_cb=None, done_cb=None):
        """ A synchronous version of the gaze function.

        No ActionHandle is returned because the action completes before it returns.
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (Entity, float, (None, Callable), (None, Callable)),
                            target, speed, feedback_cb, done_cb)

        self.gaze(target, speed, feedback_cb, done_cb)
        self.gaze_client.wait_for_result()

    def gesture(self, gesture, duration=Default(), target=None, feedback_cb=None, done_cb=None):
        """  Makes the robot perform a gesture, e.g. make a robot wave its left arm.

         The default values for the duration of each gesture are specified in the robots IGesture enumeration definition.

        :param gesture: the gesture to perform, an IGesture Python enumeration member e.g. for Nao, Gesture.WaveLArm.
        :type gesture: IGesture

        :param duration: the length of time the gesture plays for (seconds). If Default then the gesture is performed for
        its default duration. Use positive infinity for a never ending gesture, hint: float('inf')
        :type duration: float, Default

        :param target: the target to orient the gesture toward
        :type target: None, Entity

        :param feedback_cb: a function that will be called when the state of the gesture action changes. The function
        should have one parameter, which will be filled with a GestureActionFeedback instance.
        :type feedback_cb: Callable

        :param done_cb: a function that will be called when the gesture action finishes.
        :type done_cb: Callable

        :return: an action handle to keep track of the action
        :rtype: MultiGoalActionHandle
        :raises TypeError: gesture is not an IGesture member, duration is not a float or Default, target is not None or
        an Entity, feedback_cb is not None or Callable, done_cb is not None or Callable
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (IGesture, (Default, float), (None, Entity), (None, Callable), (None, Callable)),
                            gesture, duration, feedback_cb, done_cb)

        if Robot.is_action_runnable(self.gesture_client, GestureAction.__name__, self.gesture_found):
            if target is not None:
                World().add_to_world(target)

            goal = GestureGoal(gesture, duration, target) #TODO: check goal.target = target.get_id()
            gh = self.gesture_client.send_goal(goal, feedback_cb, done_cb)
            ah = MultiGoalActionHandle(self.gesture_client, gh)
            return ah

    def gesture_and_wait(self, gesture, duration=Default, target=None, feedback_cb=None, done_cb=None):
        """ A synchronous version of the gesture function.

        No ActionHandle is returned because the action completes before it returns.
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (IGesture, (Default, float), (None, Entity), (None, Callable), (None, Callable)),
                            gesture, duration, feedback_cb, done_cb)

        ah = self.gesture(gesture, duration, target, feedback_cb, done_cb)
        self.gesture_client.wait_for_result(ah.goal_handle)

    def expression(self, expression, intensity=Default(), speed=Default(), duration=Default(), feedback_cb=None, done_cb=None):
        """ Makes the robot perform a facial expression, e.g. smile.

        The default values for the intensity, speed and duration of each gesture are specified in the robots IExpression
        enumeration definition.

        This function is asynchronous and returns an ActionHandle.

        :param expression: The type of expression to perform, an IExpression enumeration member (e.g.for Zeno, Expression.Smile).
        :type expression: IExpression subclass member

        :param intensity: The strength of the expression e.g. a smile with an intensity of 1.0 is the largest possible smile
        (normalised between 0.0 < speed <= 1.0).
        :type intensity: float, Default

        :param speed: how fast the expression is performed, e.g. a smile expression with a speed of 1.0 is
        performed at the fastest possible speed (normalized between 0.0 <= speed <= 1.0). If Default() then the
        expression is performed at its default speed.
        :type speed: float, Default

        :param duration: how long the expression lasts (seconds). If Default() then the expression is performed for its
        default duration. Use positive infinity for a never ending expression, hint: float('inf')
        :type duration: float, Default

        :param feedback_cb: a function that will be called when the state of the expression action changes. The function
        should have one parameter, which will be filled with a ExpressionActionFeedback instance.
        :type feedback_cb: Callable

        :param done_cb: a function that will be called when the expression action finishes.
        :type done_cb: Callable

        :return: an action handle to keep track of the action
        :rtype: MultiGoalActionHandle
        :raises TypeError: expression is not an IExpression member, intensity is not a float or Default, speed is not a
        float or Default, duration is not a float or Default, feedback_cb is not None or Callable, done_cb is not None or Callable
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (IExpression, (Default, float), (Default, float), (Default, float), (None, Callable), (None, Callable)),
                            expression, intensity, speed, duration, feedback_cb, done_cb)

        if Robot.is_action_runnable(self.expression_client, ExpressionAction.__name__, self.expression_found):
            goal = ExpressionGoal(expression, intensity, speed, duration)
            gh = self.expression_client.send_goal(goal, feedback_cb, done_cb)
            ah = MultiGoalActionHandle(self.expression_client, gh)
            return ah

    def expression_and_wait(self, expression, intensity=Default, speed=Default, duration=Default, feedback_cb=None, done_cb=None):
        """ A synchronous version of the expression function.

        No ActionHandle is returned because the action completes before it returns.
        """

        TypeChecker.accepts(inspect.currentframe().f_code.co_name,
                            (IExpression, (Default, float), (Default, float), (Default, float), (None, Callable), (None, Callable)),
                            expression, intensity, speed, duration, feedback_cb, done_cb)

        ah = self.expression(expression, intensity, speed, duration, feedback_cb, done_cb)
        self.expression_client.wait_for_result(ah.goal_handle)

    def simultaneously(self, *goals):
        action_handles = []

        with self.lock:
            for goal in goals:
                if isinstance(goal, SayGoal):
                    if Robot.is_action_runnable(self.say_client, SayAction.__name__, self.say_found):
                        self.say_client.send_goal(goal)
                        ah = SingleGoalActionHandle(self.say_client)
                        action_handles.append(ah)

                elif isinstance(goal, GazeGoal):
                    if Robot.is_action_runnable(self.gaze_client, GazeAction.__name__, self.gaze_found):
                        self.gaze_client.send_goal(goal)
                        ah = SingleGoalActionHandle(self.gaze_client)
                        action_handles.append(ah)

                elif isinstance(goal, GestureGoal):
                    if Robot.is_action_runnable(self.gesture_client, GestureAction.__name__, self.gesture_found):
                        gh = self.gesture_client.send_goal(goal)
                        ah = MultiGoalActionHandle(self.gesture_client, gh)
                        action_handles.append(ah)

                elif isinstance(goal, ExpressionGoal):
                    if Robot.is_action_runnable(self.expression_client, ExpressionAction.__name__, self.expression_found):
                        gh = self.expression_client.send_goal(goal)
                        ah = MultiGoalActionHandle(self.expression_client, gh)
                        action_handles.append(ah)
                else:
                    raise TypeError('robot.simultaneously parameter goals has a goal with a unsupported type {0}, at index {1}. Should be one of: TextToSpeechGoal, TargetGoal, ExpressionGoal or GestureGoal'.format(type(goal), goals.index(goal)))

        return action_handles

    # TODO: simultaneously action handle
    # TODO: consecutively method and action handle

    # Wait for one or more goals to finish
    def wait(self, *action_handles):
        for ah in action_handles:
            #TODO: ParamAssertions.assert_types(self.wait, ah, IActionHandle)
            ah.wait_for_result()

    # Cancel one or more goals
    def cancel(self, *action_handles):
        for ah in action_handles:
            #TODO: ParamAssertions.assert_types(self.wait, ah, IActionHandle)
            ah.cancel_action()

    @staticmethod
    def is_action_runnable(action_client, action_type_name, action_found):
        if action_client is None:
            rospy.logerr("Cannot do {0} action because this action hasn't been configured for your robot. Please define "
                         "the {0} action and the namespace of its action server in your "
                         "robots yaml configuration file".format(action_type_name))
            return False

        elif not action_found:
            rospy.logerr("Cannot do {0} action because the {0} action server could not be found. Please check that "
                         "the {0} action server with the namespace {1} is running. "
                         "Hint: rostopic list | grep {1}".format(action_type_name, action_client.action_client.ns))
            return False

        return True

    @staticmethod
    def is_service_callable(service_proxy, service_type_name, service_found):
        if service_proxy is None:
            rospy.logerr("Cannot call {0} service because this service hasn't been defined for your robot. Please define "
                         "the {0} service and its name in your robots yaml configuration file".format(service_type_name))
            return False

        elif not service_found:
            rospy.logerr("Cannot call {0} service because the {0} service could not be found. Please check that "
                         "the {0} service with the name {1} is running. "
                         "Hint: rosservice list | grep {1}".format(service_type_name, service_proxy.name))
            return False

        return True

    @staticmethod
    def wait_for_action_server(action_client, action_type_name):
        connected = action_client.wait_for_server(timeout=rospy.Duration.from_sec(Robot.SERVER_TIMEOUT))

        if not connected:
            rospy.logerr("{0} actions are disabled because I could not connect to the {0} action server. Please "
                         "check that the {0} action server with the namespace {1} is running. "
                         "Hint: rostopic list | grep {1}".format(action_type_name, action_client.action_client.ns))

        return connected

    @staticmethod
    def wait_for_service(service_proxy, service_type_name):
        connected = service_proxy.wait_for_service(timeout=rospy.Duration.from_sec(Robot.SERVER_TIMEOUT))

        if not connected:
            rospy.logerr("The {0} service is disabled because I could not connect to the {0} service. Please "
                         "check that the {0} service with the namespace {1} is running. "
                         "Hint: rosservice list | grep {1}".format(service_type_name, service_proxy.name))

        return connected
コード例 #3
0
class TestMultiGoalActionClient(TestCase):
    ACTION_SERVER_NAME = "test_action_server"

    def setUp(self):
        global count
        rospy.init_node("test", anonymous=True)
        self.action_server = TimerActionServer(
            TestMultiGoalActionClient.ACTION_SERVER_NAME + "_" + str(count))
        self.action_server.start_server()
        self.client = MultiGoalActionClient(
            TestMultiGoalActionClient.ACTION_SERVER_NAME + "_" + str(count),
            TimerAction)
        self.client.wait_for_server()
        count += 1

    def tearDown(self):
        self.client.stop()

    def test_wait_for_server(self):
        started1 = self.client.wait_for_server(
            timeout=rospy.Duration.from_sec(1.0))

        client2 = MultiGoalActionClient("i_dont_exist", TimerAction)
        started2 = client2.wait_for_server(
            timeout=rospy.Duration.from_sec(1.0))

        self.assertEqual(started1, True)
        self.assertEqual(started2, False)

    def test_send_goal(self):
        timer = TimerGoal()
        timer.duration = 0.5
        gh1 = self.client.send_goal(timer)
        success = self.client.wait_for_result(
            gh1, timeout=rospy.Duration.from_sec(0.6))
        self.assertEqual(success, True)

    def test_get_result(self):
        timer = TimerGoal()
        timer.duration = 0.5
        gh1 = self.client.send_goal(timer)
        self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(0.55))
        result = self.client.get_result(gh1)

        self.assertEqual(isinstance(result, TimerResult), True)
        self.assertEqual(result.success, True)

    def test_send_two_goals_serial(self):
        # First goal
        timer1 = TimerGoal()
        timer1.duration = 0.5
        gh1 = self.client.send_goal(timer1)
        success1 = self.client.wait_for_result(
            gh1, timeout=rospy.Duration.from_sec(0.55))

        # Second goal
        timer2 = TimerGoal()
        timer2.duration = 0.5
        gh2 = self.client.send_goal(timer2)
        success2 = self.client.wait_for_result(
            gh2, timeout=rospy.Duration.from_sec(0.6))

        self.assertEqual(success1, True)
        self.assertEqual(success2, True)

    def test_send_two_goals_parallel(self):
        timer1 = TimerGoal()
        timer2 = TimerGoal()
        timer1.duration = 1.0
        timer2.duration = 1.0

        start = time.time()
        # Send both goals
        gh1 = self.client.send_goal(timer1)
        gh2 = self.client.send_goal(timer2)
        result1 = self.client.wait_for_result(
            gh1, timeout=rospy.Duration.from_sec(1.1))
        result2 = self.client.wait_for_result(
            gh2, timeout=rospy.Duration.from_sec(1.1))
        end = time.time()
        duration = end - start

        self.assertEqual(result1, True)
        self.assertEqual(result2, True)
#        self.assertAlmostEqual(duration, 1.0, places=1)

    def test_get_goal_id(self):
        timer = TimerGoal()
        timer.duration = 0.1
        gh1 = self.client.send_goal(timer)
        goal_id = self.client.goal_id(gh1)
        self.assertIsNotNone(goal_id)

    def test_is_tracking_goal(self):
        timer = TimerGoal()
        timer.duration = 0.1
        gh1 = self.client.send_goal(timer)
        tracking = self.client.is_tracking_goal(gh1)
        self.assertEqual(tracking, True)

    def test_get_action_handle(self):
        timer = TimerGoal()
        timer.duration = 1.0
        gh1 = self.client.send_goal(timer)
        time.sleep(0.5)
        action_handle = self.client.get_action_handle(gh1)
        self.assertEqual(isinstance(action_handle, ActionHandle), True)

    def test_get_state(self):
        timer = TimerGoal()
        timer.duration = 1.0
        gh1 = self.client.send_goal(timer)
        pending = self.client.get_state(gh1)
        time.sleep(0.5)
        active = self.client.get_state(gh1)
        self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(1.1))
        succeeded = self.client.get_state(gh1)

        # gh2 = self.client.send_goal(timer)
        # time.sleep(0.5)
        # self.client.cancel_goal(gh2)
        #preeempted = self.client.get_state(gh2)

        self.assertEqual(pending, GoalStatus.PENDING)
        self.assertEqual(active, GoalStatus.ACTIVE)
        self.assertEqual(succeeded, GoalStatus.SUCCEEDED)
        # self.assertEqual(preeempted, GoalStatus)

    def test_cancel_goal(self):
        timer1 = TimerGoal()
        timer1.duration = 5.0

        gh1 = self.client.send_goal(timer1)
        pending = self.client.get_state(gh1)
        time.sleep(0.5)
        active = self.client.get_state(gh1)
        self.client.cancel_goal(gh1)
        time.sleep(0.5)
        preempted = self.client.get_state(gh1)

        self.assertEqual(GoalStatus.PENDING, pending)
        self.assertEqual(GoalStatus.ACTIVE, active)
        self.assertEqual(GoalStatus.PREEMPTED, preempted)

    def test_cancel_all_goals(self):
        timer1 = TimerGoal()
        timer2 = TimerGoal()
        timer1.duration = 1.0
        timer2.duration = 1.0

        gh1 = self.client.send_goal(timer1)
        gh2 = self.client.send_goal(timer2)
        time.sleep(0.5)
        self.client.cancel_all_goals()
        time.sleep(0.5)

        self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh1))
        self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh2))

    def test_cancel_goals_at_and_before_time(self):
        before_time1 = TimerGoal()
        before_time2 = TimerGoal()
        before_time1.duration = 5.0
        before_time2.duration = 5.0
        gh1 = self.client.send_goal(before_time1)
        gh2 = self.client.send_goal(before_time2)
        cancel_time = rospy.Time().now()

        after_time_1 = TimerGoal()
        after_time_1.duration = 1.0
        gh3 = self.client.send_goal(after_time_1)
        time.sleep(0.5)
        self.client.cancel_goals_at_and_before_time(cancel_time)

        success = self.client.wait_for_result(
            gh3, timeout=rospy.Duration.from_sec(1.1))

        self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh1))
        self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state(gh2))
        self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state(gh3))
        self.assertEqual(success, True)

    def test_done_callback(self):
        timer = TimerGoal()
        timer.duration = 0.5
        mock = Mock()
        gh1 = self.client.send_goal(timer, done_cb=mock)
        time.sleep(1.0)
        mock.assert_called_once_with(gh1)

    def test_active_callback(self):
        timer = TimerGoal()
        timer.duration = 0.5
        mock = Mock()
        gh1 = self.client.send_goal(timer, active_cb=mock)
        self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(1.1))
        mock.assert_called_once_with(gh1)

    def test_feedback_callback(self):
        timer = TimerGoal()
        timer.duration = 1.0
        mock = Mock()
        gh1 = self.client.send_goal(timer, feedback_cb=mock)
        self.client.wait_for_result(gh1, timeout=rospy.Duration.from_sec(1.1))
        mock.assert_called_once_with(gh1, TimerFeedback(current_time=10))
コード例 #4
0
ファイル: robot.py プロジェクト: jdddog/hri
class Robot(Entity):
    ENTITY_TYPE = "robot"

    def __init__(self, expression_enum, gesture_enum):
        Entity.__init__(self, Robot.ENTITY_TYPE, Robot.ENTITY_TYPE + str(1), None)
        self.expression_enum = expression_enum
        self.gesture_enum = gesture_enum

        self.gaze_client = actionlib.SimpleActionClient("gaze", TargetAction)
        self.gaze_found = False

        self.expression_client = MultiGoalActionClient("expression", ExpressionAction)
        self.expression_found = False

        self.tts_client = actionlib.SimpleActionClient("text_to_speech", TextToSpeechAction)
        self.tts_found = False

        self.gesture_client = MultiGoalActionClient("gesture", GestureAction)
        self.gesture_found = False

        self.tts_duration_srv = rospy.ServiceProxy("tts_subsentence_duration", TextToSpeechSubsentenceDuration)
        self.tts_duration_found = False

        self.event = None
        self.action_handles = []
        self.say_to_plan = SayToPlan(self)
        self.gaze_ah = None
        self.say_ah = None

        self.lock = threading.RLock()
        self.listen_cb = None
        self.listen_sub = None

    def register_listen_callback(self, callback):
        self.listen_cb = callback
        self.listen_sub = rospy.Subscriber("itf_listen", String, self.__listen, queue_size=10)

    def __listen(self, msg):

        if msg.data == "BADINPUT":
            text = None
        else:
            text = msg.data

        self.listen_cb(text)

    # Text to speech
    def say(self, text):
        if not self.tts_found:
            self.wait_for_action_servers(self.tts_client)
            self.tts_found = True

        ParamFormatting.assert_types(self.say, text, str)
        goal = TextToSpeechGoal()
        goal.sentence = text
        self.tts_client.send_goal(goal, feedback_cb=self.say_feedback, done_cb=self.say_done)
        ah = SingleGoalActionHandle(self.tts_client)
        self.say_ah = ah
        self.add_action_handle(ah)
        return ah

    def say_and_wait(self, text):
        self.say(text)
        self.tts_client.wait_for_result()

    def say_done(self, state, result):
        with self.lock:
            self.remove_action_handle(self.say_ah)
            self.say_ah = None

    # Gaze
    def gaze(self, target, speed=0.5):
        if not self.gaze_found:
            self.wait_for_action_servers(self.gaze_client)
            self.gaze_found = True

        ParamFormatting.assert_types(self.gaze, target, Entity)
        ParamFormatting.assert_types(self.gaze, speed, float)
        ParamFormatting.assert_range(self.gaze, speed, 0.0, 1.0)

        World().add_to_world(target)
        goal = TargetGoal()
        goal.target = target.get_id()
        goal.speed = speed
        goal.acceleration = 0.3

        self.gaze_client.send_goal(goal, feedback_cb=self.gaze_feedback, done_cb=self.gaze_done)
        ah = SingleGoalActionHandle(self.gaze_client)
        self.gaze_ah = ah
        self.add_action_handle(ah)
        return ah

    def gaze_and_wait(self, target, speed=0.5, timeout=rospy.Duration()):
        self.gaze(target, speed)
        self.gaze_client.wait_for_result(timeout)

    def gaze_feedback(self, feedback):
        pass

    def gaze_done(self, state, result):
        with self.lock:
            self.remove_action_handle(self.gaze_ah)
            self.gaze_ah = None

    # Blinking
    def blink(self, blink_duration, blink_rate_mean, blink_rate_sd):
        """
        :param blink_duration:
        :param blink_rate_mean:
        :param blink_rate_sd:
        :return:
        """
        raise NotImplementedError("Please implement the blink method")

    # Facial expressions
    def expression(self, expression, intensity=None, speed=None, duration=None):
        if not self.expression_found:
            self.wait_for_action_servers(self.expression_client)
            self.expression_found = True

        ParamFormatting.assert_types(self.expression, expression, IExpression)

        goal = ExpressionGoal()
        goal.expression = expression.name

        if intensity is None:
            goal.intensity = -1
        else:
            ParamFormatting.assert_types(self.expression, intensity, float)
            ParamFormatting.assert_range(self.expression, intensity, 0.0, 1.0)
            goal.intensity = intensity

        if speed is None:
            goal.speed = -1
        else:
            ParamFormatting.assert_types(self.expression, speed, float)
            ParamFormatting.assert_range(self.expression, speed, 0.0, 1.0)
            goal.speed = speed

        if duration is None:
            goal.duration = -1
        else:
            ParamFormatting.assert_types(self.expression, duration, float)
            ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
            goal.duration = duration

        gh = self.expression_client.send_goal(goal, done_cb=self.expression_done)
        ah = MultiGoalActionHandle(self.expression_client, gh)
        self.add_action_handle(ah)
        return ah

    def expression_and_wait(self, expression, intensity=None, speed=None, duration=None, timeout=rospy.Duration()):
        ah = self.expression(expression, intensity, speed, duration)
        self.expression_client.wait_for_result(ah.goal_handle, timeout)

    def expression_done(self, goal_handle):
        with self.lock:
            ah = self.get_action_handle(goal_handle)
            self.remove_action_handle(ah)

    # Gestures
    def gesture(self, gesture, target=None, duration=None):
        if not self.gesture_found:
            self.wait_for_action_servers(self.gesture_client)
            self.gesture_found = True

        ParamFormatting.assert_types(self.gesture, gesture, IGesture)

        goal = GestureGoal()
        goal.gesture = gesture.name

        if target is None:
            goal.target = ""
        else:
            World().add_to_world(target)
            ParamFormatting.assert_types(self.gesture, target, Entity)
            goal.target = target.get_id()

        if duration is None:
            goal.duration = -1
        else:
            ParamFormatting.assert_types(self.expression, duration, float)
            ParamFormatting.assert_greater_than(self.expression, duration, 0.0)
            goal.duration = duration

        gh = self.gesture_client.send_goal(goal, done_cb=self.gesture_done)

        ah = MultiGoalActionHandle(self.gesture_client, gh)
        self.add_action_handle(ah)
        return ah

    def gesture_and_wait(self, gesture, target=None, duration=None, timeout=rospy.Duration()):
        ah = self.gesture(gesture, target, duration)
        self.gesture_client.wait_for_result(ah.goal_handle, timeout)

    def gesture_done(self, goal_handle):
        with self.lock:
            ah = self.get_action_handle(goal_handle)
            self.remove_action_handle(ah)

    # Speaking, gazing and gesturing simultaneously
    def say_to(self, text, audience):
        if not self.tts_duration_found:
            self.wait_for_services(self.tts_duration_srv)
            self.tts_duration_found = True

        ParamFormatting.assert_types(self.say_to, text, str)
        ParamFormatting.assert_types(self.say_to, audience, Entity, Query, list)

        self.say_to_plan.parse_parameters(
            text, audience, self.expression_enum, self.gesture_enum, self.tts_duration_srv
        )

        ah = self.say_to_plan.execute()
        self.add_action_handle(ah)
        return ah

    def say_to_and_wait(self, text, audience):
        ah = self.say_to(text, audience)
        self.wait(ah)

    def say_feedback(self, feedback):
        if feedback.current_word_index in self.say_to_plan.gaze_change_locations:
            if isinstance(self.say_to_plan.audience, Entity):
                person = self.say_to_plan.audience
                self.say_to_plan.current_gazee = person
                self.say_to_plan.gaze_ah = self.gaze(person.head)

            elif isinstance(self.say_to_plan.audience, Query):
                people = self.say_to_plan.audience.execute()

                if len(people) > 1:
                    # if self.say_to_plan.current_gazee in people:
                    #    people.remove(self.say_to_plan.current_gazee)

                    person = random.choice(people)
                    self.say_to_plan.current_gazee = person
                    self.say_to_plan.gaze_ah = self.gaze(person.head)
                elif len(people) == 1:
                    person = people[0]
                    self.say_to_plan.current_gazee = person
                    self.say_to_plan.gaze_ah = self.gaze(person.head)

        if feedback.current_word_index in self.say_to_plan.expression_lookup:
            expression = self.say_to_plan.expression_lookup[feedback.current_word_num]
            ahs = self.do(expression)
            self.say_to_plan.add_action_handle(ahs[0])

        if feedback.current_word_index in self.say_to_plan.gesture_lookup:
            gesture = self.say_to_plan.gesture_lookup[feedback.current_word_index]
            ahs = self.do(gesture)
            self.say_to_plan.add_action_handle(ahs[0])

    def add_action_handle(self, action_handle):
        with self.lock:
            ParamFormatting.assert_types(self.add_action_handle, action_handle, IActionHandle)
            self.action_handles.append(action_handle)

    def get_action_handle(self, goal_handle):
        with self.lock:
            for ah in self.action_handles:
                if isinstance(ah, MultiGoalActionHandle):
                    if ah.goal_handle == goal_handle:
                        return ah

    def remove_action_handle(self, action_handle):
        with self.lock:
            ParamFormatting.assert_types(self.remove_action_handle, action_handle, IActionHandle)
            if action_handle in self.action_handles:
                self.action_handles.remove(action_handle)

    def do(self, *goals):
        action_handles = []

        with self.lock:
            for goal in goals:
                if isinstance(goal, TextToSpeechGoal):
                    self.tts_client.send_goal(goal)
                    ah = SingleGoalActionHandle(self.tts_client)
                    self.add_action_handle(ah)
                    action_handles.append(ah)

                elif isinstance(goal, TargetGoal):
                    self.gaze_client.send_goal(goal)
                    ah = SingleGoalActionHandle(self.gaze_client)
                    self.add_action_handle(ah)
                    action_handles.append(ah)

                elif isinstance(goal, ExpressionGoal):
                    gh = self.expression_client.send_goal(goal)
                    ah = MultiGoalActionHandle(self.expression_client, gh)
                    self.add_action_handle(ah)
                    action_handles.append(ah)

                elif isinstance(goal, GestureGoal):
                    gh = self.gesture_client.send_goal(goal)
                    ah = MultiGoalActionHandle(self.gesture_client, gh)
                    self.add_action_handle(ah)
                    action_handles.append(ah)
                else:
                    raise TypeError(
                        "robot.do parameter goals has a goal with a unsupported type {0}, at index {1}. Should be one of: TextToSpeechGoal, TargetGoal, ExpressionGoal or GestureGoal".format(
                            type(goal), goals.index(goal)
                        )
                    )

        return action_handles

    # Wait for one or more goals to finish
    def wait(self, *action_handles):
        for ah in action_handles:
            ParamFormatting.assert_types(self.wait, ah, IActionHandle)
            ah.wait_for_result()
            self.remove_action_handle(action_handle=ah)

    # Cancel one or more goals
    def cancel(self, *action_handles):
        for ah in action_handles:
            ParamFormatting.assert_types(self.wait, ah, IActionHandle)
            ah.cancel_action()
            self.remove_action_handle(action_handle=ah)

    def default_tf_frame_id(self):
        raise NotImplementedError("Please implement this method")

    def tf_frame_id(self):
        raise NotImplementedError("Please implement this method")

    # Wait for a period of time
    def wait_for_period(self, period):
        self.event = threading.Event()
        self.event.wait(timeout=period)

    def cancel_wait_for_period(self):
        if self.event is not None:
            self.event.set()