Ejemplo n.º 1
0
class IExpressionActionServer(object):
    __metaclass__ = abc.ABCMeta

    def __init__(self):
        self.action_server = MultiGoalActionServer('expression',
                                                   ExpressionAction,
                                                   auto_start=False)
        self.action_server.register_goal_callback(self.__goal_callback)
        self.action_server.register_preempt_callback(self.__preempt_callback)

    def start(self):
        self.action_server.start()

    def __goal_callback(self, goal_handle):
        goal = goal_handle.get_goal()
        self.start_expression(goal)
        rospy.loginfo("Expression received id: %s, name: %s",
                      goal_handle.get_goal_id().id, goal.type)

    def __preempt_callback(self, goal_handle):
        goal = goal_handle.get_goal()
        self.stop_expression(goal)
        rospy.loginfo("Expression preempted id: %s, name: %s",
                      goal_handle.get_goal_id().id, goal.type)

    @abc.abstractmethod
    def start_expression(self, goal):
        """ Start your expression """
        return

    @abc.abstractmethod
    def stop_expression(self, goal):
        """ Stop the expression corresponding to this goal """
        return
Ejemplo n.º 2
0
class IExpressionActionServer(object):
    __metaclass__ = abc.ABCMeta

    def __init__(self):
        self.action_server = MultiGoalActionServer('expression', ExpressionAction, auto_start=False)
        self.action_server.register_goal_callback(self.__goal_callback)
        self.action_server.register_preempt_callback(self.__preempt_callback)

    def start(self):
        self.action_server.start()

    def __goal_callback(self, goal_handle):
        goal = goal_handle.get_goal()
        self.start_expression(goal)
        rospy.loginfo("Expression received id: %s, name: %s", goal_handle.get_goal_id().id, goal.type)

    def __preempt_callback(self, goal_handle):
        goal = goal_handle.get_goal()
        self.stop_expression(goal)
        rospy.loginfo("Expression preempted id: %s, name: %s", goal_handle.get_goal_id().id, goal.type)

    @abc.abstractmethod
    def start_expression(self, goal):
        """ Start your expression """
        return

    @abc.abstractmethod
    def stop_expression(self, goal):
        """ Stop the expression corresponding to this goal """
        return
class MiniHeadExpressionServer(IExpressionActionServer):

    def __init__(self):
        self.action_server = MultiGoalActionServer('expression', ExpressionAction, auto_start=False)
        self.action_server.register_goal_callback(self.goal_callback)
        self.motor_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=10)
        self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range', MotorRange)
        self.rate = rospy.Rate(10)

    def start(self):
        self.action_server.start()

    def goal_callback(self, goal_handle):
        #self.action_server.set_accepted(goal_handle)
        new_goal = goal_handle.get_goal()
        rospy.loginfo('Accepted new goal: {0}'.format(new_goal))

        found = False
        for name, member in Expression.__members__.items():
            if name == new_goal.expression:
                found = True

        if not found:
            rospy.logerr('{0} is not a valid expression. Valid expressions are: {1}'.format(new_goal.expression, Expression))
            self.action_server.set_aborted(goal_handle)
            return

        expression_thread = threading.Thread(target=self.start_expression, args=[goal_handle])
        expression_thread.run()

    def start_expression(self, goal_handle):
        goal = goal_handle.get_goal()

        expression = Expression[goal.expression]
        speed = goal.speed
        intensity = goal.intensity
        duration = goal.duration

        # Checking for default values (-1 means nothing was specified, hence we should use the default values)
        if speed == -1:
            speed = expression.default_speed

        if intensity == -1:
            intensity = expression.default_intensity

        if duration == -1 and expression.default_duration is not None:
            duration = expression.default_duration

        if expression in [Expression.smile, Expression.frown_mouth]:
            joint_name = 'smile_joint'
        elif expression in [Expression.open_mouth]:
            joint_name = 'jaw_joint'
        elif expression in [Expression.frown]:
            joint_name = 'brow_joint'

        negative = True
        if expression in [Expression.smile, Expression.open_mouth]:
            negative = False

        response = self.motor_range_srv(joint_name)

        msg = MotorCommand()
        msg.joint_name = joint_name

        min_range = min([response.min, response.max])
        max_range = max([response.min, response.max])

        if negative:
            msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range)
        else:
            msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range)

        msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5)
        msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4)

        start = time.time()
        while not rospy.is_shutdown() and not self.action_server.is_preempt_requested(goal_handle) and (time.time() - start) < duration:
            if duration >= 0.0 and (time.time() - start) < duration:
                break

            self.motor_pub.publish(msg)
            self.rate.sleep()

        if not self.action_server.is_preempt_requested(goal_handle):
            self.action_server.set_succeeded(goal_handle)

    def stop_expression(self, goal_handle):
        raise NotImplementedError('Please implement this method')
Ejemplo n.º 4
0
class IGestureActionServer():
    __metaclass__ = abc.ABCMeta

    def __init__(self, gesture_enum):
        self.node_name = "gesture"
        self.gesture_enum = gesture_enum
        self.gesture_handle_lookup = {}
        self.action_server = MultiGoalActionServer(self.node_name,
                                                   GestureAction,
                                                   auto_start=False)
        self.action_server.register_goal_callback(self.__goal_callback)
        self.action_server.register_preempt_callback(self.__preempt_callback)

    def start(self):
        self.action_server.start()
        rospy.loginfo('GestureActionServer started')

    def is_valid_gesture(self, gesture_name):
        found = False
        for name, member in self.gesture_enum.__members__.items():
            if name == gesture_name:
                return True

        if not found:
            rospy.logerr(
                '{0} is not a valid gesture. Valid gestures are: {1}'.format(
                    gesture_name, self.gesture_enum.__members__.items()))
            return False

    def __goal_callback(self, goal_handle):
        new_goal = goal_handle.get_goal()
        self.start_gesture(goal_handle)
        rospy.loginfo("Gesture received id: %s, name: %s",
                      goal_handle.get_goal_id().id, new_goal.gesture)

    def __preempt_callback(self, goal_handle):
        self.cancel_gesture(goal_handle)
        gesture_handle = self.get_gesture_handle(goal_handle)
        self.remove_gesture_handle(gesture_handle)
        rospy.loginfo("Gesture preempted id: %s, name: %s",
                      goal_handle.get_goal_id().id,
                      goal_handle.get_goal().gesture)

    @abc.abstractmethod
    def start_gesture(self, goal_handle):
        """ Start your gesture. """
        return

    @abc.abstractmethod
    def cancel_gesture(self, goal_handle):
        """ Cancel gesture if it is currently running. """
        return

    def send_feedback(self, goal_handle, distance_to_target):
        """ Call this method to send feedback about the distance to the target """
        feedback = GestureActionFeedback()
        feedback.distance_to_target = distance_to_target
        self.action_server.publish_feedback(goal_handle, self.feedback)
        rospy.loginfo(
            "Gesture feedback id: %s, name: %s, distance_to_target: $s",
            goal_handle.get_goal_id().id,
            goal_handle.get_goal().type, distance_to_target)

    def set_succeeded(self, goal_handle):
        """ Call this method when the gesture has finished """
        self.action_server.set_succeeded(goal_handle)
        gesture_handle = self.get_gesture_handle(goal_handle)
        self.remove_gesture_handle(gesture_handle)
        rospy.loginfo("Gesture finished id: %s, name: %s",
                      goal_handle.get_goal_id().id,
                      goal_handle.get_goal().gesture)

    def set_aborted(self, goal_handle):
        self.action_server.set_aborted()
        gesture_handle = self.get_gesture_handle(goal_handle)
        self.remove_gesture_handle(gesture_handle)

    def add_gesture_handle(self, gesture_handle):
        self.gesture_handle_lookup[gesture_handle.goal_id()] = gesture_handle

    def remove_gesture_handle(self, gesture_handle):
        self.gesture_handle_lookup.pop(gesture_handle.goal_id())

    def get_gesture_handle(self, goal_handle):
        return self.gesture_handle_lookup[goal_handle.get_goal_id().id]
class MiniHeadExpressionServer(IExpressionActionServer):
    def __init__(self):
        self.action_server = MultiGoalActionServer('expression',
                                                   ExpressionAction,
                                                   auto_start=False)
        self.action_server.register_goal_callback(self.goal_callback)
        self.motor_pub = rospy.Publisher('pololu/command',
                                         MotorCommand,
                                         queue_size=10)
        self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range',
                                                  MotorRange)
        self.rate = rospy.Rate(10)

    def start(self):
        self.action_server.start()

    def goal_callback(self, goal_handle):
        #self.action_server.set_accepted(goal_handle)
        new_goal = goal_handle.get_goal()
        rospy.loginfo('Accepted new goal: {0}'.format(new_goal))

        found = False
        for name, member in Expression.__members__.items():
            if name == new_goal.expression:
                found = True

        if not found:
            rospy.logerr(
                '{0} is not a valid expression. Valid expressions are: {1}'.
                format(new_goal.expression, Expression))
            self.action_server.set_aborted(goal_handle)
            return

        expression_thread = threading.Thread(target=self.start_expression,
                                             args=[goal_handle])
        expression_thread.run()

    def start_expression(self, goal_handle):
        goal = goal_handle.get_goal()

        expression = Expression[goal.expression]
        speed = goal.speed
        intensity = goal.intensity
        duration = goal.duration

        # Checking for default values (-1 means nothing was specified, hence we should use the default values)
        if speed == -1:
            speed = expression.default_speed

        if intensity == -1:
            intensity = expression.default_intensity

        if duration == -1 and expression.default_duration is not None:
            duration = expression.default_duration

        if expression in [Expression.smile, Expression.frown_mouth]:
            joint_name = 'smile_joint'
        elif expression in [Expression.open_mouth]:
            joint_name = 'jaw_joint'
        elif expression in [Expression.frown]:
            joint_name = 'brow_joint'

        negative = True
        if expression in [Expression.smile, Expression.open_mouth]:
            negative = False

        response = self.motor_range_srv(joint_name)

        msg = MotorCommand()
        msg.joint_name = joint_name

        min_range = min([response.min, response.max])
        max_range = max([response.min, response.max])

        if negative:
            msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range)
        else:
            msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range)

        msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5)
        msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4)

        start = time.time()
        while not rospy.is_shutdown(
        ) and not self.action_server.is_preempt_requested(goal_handle) and (
                time.time() - start) < duration:
            if duration >= 0.0 and (time.time() - start) < duration:
                break

            self.motor_pub.publish(msg)
            self.rate.sleep()

        if not self.action_server.is_preempt_requested(goal_handle):
            self.action_server.set_succeeded(goal_handle)

    def stop_expression(self, goal_handle):
        raise NotImplementedError('Please implement this method')
Ejemplo n.º 6
0
class IGestureActionServer():
    __metaclass__ = abc.ABCMeta

    def __init__(self, gesture_enum):
        self.node_name = "gesture"
        self.gesture_enum = gesture_enum
        self.gesture_handle_lookup = {}
        self.action_server = MultiGoalActionServer(self.node_name, GestureAction, auto_start=False)
        self.action_server.register_goal_callback(self.__goal_callback)
        self.action_server.register_preempt_callback(self.__preempt_callback)

    def start(self):
        self.action_server.start()
        rospy.loginfo('GestureActionServer started')

    def is_valid_gesture(self, gesture_name):
        found = False
        for name, member in self.gesture_enum.__members__.items():
            if name == gesture_name:
                return True

        if not found:
            rospy.logerr('{0} is not a valid gesture. Valid gestures are: {1}'.format(gesture_name, self.gesture_enum.__members__.items()))
            return False

    def __goal_callback(self, goal_handle):
        new_goal = goal_handle.get_goal()
        self.start_gesture(goal_handle)
        rospy.loginfo("Gesture received id: %s, name: %s", goal_handle.get_goal_id().id, new_goal.gesture)

    def __preempt_callback(self, goal_handle):
        self.cancel_gesture(goal_handle)
        gesture_handle = self.get_gesture_handle(goal_handle)
        self.remove_gesture_handle(gesture_handle)
        rospy.loginfo("Gesture preempted id: %s, name: %s", goal_handle.get_goal_id().id, goal_handle.get_goal().gesture)

    @abc.abstractmethod
    def start_gesture(self, goal_handle):
        """ Start your gesture. """
        return

    @abc.abstractmethod
    def cancel_gesture(self, goal_handle):
        """ Cancel gesture if it is currently running. """
        return

    def send_feedback(self, goal_handle, distance_to_target):
        """ Call this method to send feedback about the distance to the target """
        feedback = GestureActionFeedback()
        feedback.distance_to_target = distance_to_target
        self.action_server.publish_feedback(goal_handle, self.feedback)
        rospy.loginfo("Gesture feedback id: %s, name: %s, distance_to_target: $s", goal_handle.get_goal_id().id, goal_handle.get_goal().type, distance_to_target)

    def set_succeeded(self, goal_handle):
        """ Call this method when the gesture has finished """
        self.action_server.set_succeeded(goal_handle)
        gesture_handle = self.get_gesture_handle(goal_handle)
        self.remove_gesture_handle(gesture_handle)
        rospy.loginfo("Gesture finished id: %s, name: %s",  goal_handle.get_goal_id().id, goal_handle.get_goal().gesture)

    def set_aborted(self, goal_handle):
        self.action_server.set_aborted()
        gesture_handle = self.get_gesture_handle(goal_handle)
        self.remove_gesture_handle(gesture_handle)

    def add_gesture_handle(self, gesture_handle):
        self.gesture_handle_lookup[gesture_handle.goal_id()] = gesture_handle

    def remove_gesture_handle(self, gesture_handle):
        self.gesture_handle_lookup.pop(gesture_handle.goal_id())

    def get_gesture_handle(self, goal_handle):
        return self.gesture_handle_lookup[goal_handle.get_goal_id().id]