def exec_music(goal):
r = MusicResult()
fb = MusicFeedback()
for i, f in enumerate(goal.freqs):
fb.remaining_steps = len(goal.freqs) - i
music.publish_feedback(fb)
if music.is_preempt_requested():
write.freq(0)
r.finished = False
music.set_preemtped(r)
return
write_freq(f)
rospy.sleep(1.0 if i > len(goal.durations) else goal.durations[i])
r.finished = True
music.set_succeeded(r)
def exec_music(goal):
r = MusicResult()
fb = MusicFeedback()
#input value of goal.freqs to i and input arg index of (current) goal.freqs to f
for i, f in enumerate(goal.freqs):
fb.remaining_steps = len(goal.freqs) - i
music.publish_feedback(fb)
if music.is_preempt_requested():
write_freq(0)
r.finished = False
# occur preemption then call set preempted method and stop music
music.set_preempted(r)
return
write_freq(f)
# sleep for duration
rospy.sleep(1.0 if i >= len(goal.durations) else goal.durations[i])
# execute all freq then call set_succeeded method
r.finished = True
music.set_succeeded(r)
from std_msgs.msg import UInt16
from pimouse_ros.msg import MusicAction, MusicResult, MusicFeedback
def write_freq(hz=0):
bfile = "/dev/rtbuzzer0"
try:
with open(bfile,"w") as f:
f.write(str(hz) + "\n")
except IOError:
rospy.logerr("can't write to " + bfile)
def recv_buzzer(data):
write_freq(data.data)
def exec_music(goal): pass
r = MusicResult()
fb = MusicFeedback()
for i, f in enumerate(goal.freqs):
fb.remaining_steps = len(goal.freqs) - i
if __name__== '__main__':
rospy.init_node('buzzer')
rospy.Subscriber("buzzer", UInt16, recv_buzzer)
music = actionlib.SimpleActionServer('music',MusicAction, exec_music, False)
music.start()
rospy.on_shutdown(write_freq)
rospy.spin()