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