def exec_music(goal): # pass: executing noting
r = MusicResult() #Create instance of Music Result
fb = MusicFeedback() #Create instance of Music Feedback
for i, f in enumerate(goal.freqs):
# for a, b in enumerate(list):
# a: index of element in list
# b: value of element in list
# enumerate(list): while an element can be pull
fb.remaining_steps = len(goal.freqs) - 1
#len(): obtain the length of list
music.publish_feedback(fb)
if music.is_preempt_requested(): # if the action is preempted
write_freq(0) #stop the music by frequency=0[hz]
r.finished = False # Result = false
music.set_preempted(r) #return the result
return
write_freq(f)
rospy.sleep(1.0 if i >= len(goal.durations) else goal.durations[i])
#1.0[s] sleep when duration is not specified
r.finished = True
music.set_succeeded(r)
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_preempted(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()
