class TimerActionServer(object):
def __init__(self, node_name):
self.action_server = MultiGoalActionServer(node_name, TimerAction, auto_start=False)
self.action_server.register_goal_callback(self.goal_callback)
self.action_server.register_preempt_callback(self.preempt_callback)
self.timers = {}
self.timers_lock = threading.RLock()
def start_server(self):
self.action_server.start()
def goal_callback(self, goal_handle):
print "TIMER STARTED"
new_goal = goal_handle.get_goal()
with self.timers_lock:
goal_id = goal_handle.get_goal_id().id
timer = threading.Timer(new_goal.duration, self.timer_stopped, [goal_handle])
self.timers[goal_id] = timer
timer.start()
self.action_server.publish_feedback(goal_handle, TimerFeedback(current_time=10))
def preempt_callback(self, goal_handle):
with self.timers_lock:
goal_id = goal_handle.get_goal_id().id
self.timers[goal_id].cancel()
self.action_server.set_preempted(goal_handle)
def timer_stopped(self, goal_handle):
print "TIMER STOPPED"
result = TimerResult()
result.success = True
with self.timers_lock:
goal_id = goal_handle.get_goal_id().id
self.timers.pop(goal_id)
self.action_server.set_succeeded(goal_handle, result=result, text="I succeeded!!!")
class ZoidsteinExpressionServer(object):
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 ZoidExpression.__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, ZoidExpression))
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 = ZoidExpression[goal.expression]
speed = goal.speed
intensity = goal.intensity
duration = goal.duration
if expression in [ZoidExpression.smile, ZoidExpression.frown_mouth]:
joint_name = 'smile_joint'
elif expression in [ZoidExpression.open_mouth]:
joint_name = 'jaw_joint'
elif expression in [ZoidExpression.frown]:
joint_name = 'brow_joint'
negative = True
if expression in [ZoidExpression.smile, ZoidExpression.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:
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)
class ZoidsteinExpressionServer(object):
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 ZoidExpression.__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, ZoidExpression))
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 = ZoidExpression[goal.expression]
speed = goal.speed
intensity = goal.intensity
duration = goal.duration
if expression in [ZoidExpression.smile, ZoidExpression.frown_mouth]:
joint_name = 'smile_joint'
elif expression in [ZoidExpression.open_mouth]:
joint_name = 'jaw_joint'
elif expression in [ZoidExpression.frown]:
joint_name = 'brow_joint'
negative = True
if expression in [ZoidExpression.smile, ZoidExpression.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:
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)