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 __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)
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')
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')
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]
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)