def __init__(self):
top = RosPack().get_path('mobility_games')
self.sound_folder = os.path.join(top, 'auditory/sound_files')
rospy.init_node('audio_feedback')
# Set initial values for reconfigure
self.mode = rospy.get_param("~mode", "sound")
self.sweep_tolerance = rospy.get_param("~sweep_tolerance", "2.0")
self.sweep_range = rospy.get_param("~sweep_range", "0.6")
# Set initial positions to None
self.x = None
self.y = None
self.yaw = None
self.cane_x = None
self.cane_y = None
self.cane_z = None
self.start = False
self.num_sweeps = 0
self.count_sweeps = True
self.reward_at = {}
for num in [10, 20, 50, 100, 250, 500, 1000]:
self.reward_at[num] = True
# Associate April tag ID to different wav files.
self.tag_to_music_file = {3: "ambience2.wav", 12: "generic_music.wav"}
self.tag_to_music_object = {}
self.tag_to_sound_file = {3: "ding.wav", 12: "beep.wav"}
self.tag_to_sound_object = {}
for tag_id in self.tag_to_music_file:
# Create wav object of each track in music object dictionary
self.tag_to_music_object[tag_id] = \
pw.Wav(os.path.join(self.sound_folder,
self.tag_to_music_file[tag_id]))
for tag_id in self.tag_to_sound_file:
# Create wav obejct of each sound file in sound disctionary
self.tag_to_sound_object[tag_id] = \
pw.Wav(os.path.join(self.sound_folder,
self.tag_to_music_file[tag_id]))
self.reward_sound_file = None
self.reward_sound_object = None
self.engine = pyttsx.init()
self.hasSpoken = False
# Set up dynamic reconfigure server
Server(MusicalCaneConfig, self.config_callback)
rospy.Subscriber('/tango_pose', PoseStamped, self.process_pose)
rospy.Subscriber('/cane_tip', PoseStamped, self.process_cane_tip)
rospy.Subscriber('/fisheye_undistorted/tag_detections',
AprilTagDetectionArray, self.tag_array)
def straight(self):
self.x = None
self.y = None
self.yaw = None
self.dist = None
r = rospy.Rate(10) #set the rate for loops
while not self.yaw: #not recieving data from streamer
print "NONE"
r.sleep()
pass
while not self.x: #not recieving data from streamer
print "None"
r.sleep()
pass
straightAngle = self.yaw
errorAngle = 15 * math.pi / 180
pos_x = self.x #gets the x position before you start moving
pos_y = self.y #gets the y position before you start moving
while not rospy.is_shutdown(): #tmake sure rospy not shutdown
#threshhold of the stragith angle
if abs(angle_diff(self.yaw, straightAngle)) < errorAngle:
#play the sound if you are in a straigth line
if rospy.Time.now() - self.lastBeepNoise > rospy.Duration(1):
self.beepNoise.close()
self.beepNoise = pw.Wav(
path.join(self.sound_folder, "beep3.wav"))
self.beepNoise.play()
self.lastBeepNoise = rospy.Time.now()
x = self.x - pos_x
y = self.y - pos_y
#calculate the distance between current posiiton to the starting position
distance = math.sqrt((x**2) + (y**2))
#Once you walk 1.5 meter makes you turn
if distance >= self.traveled_distance:
self.dingNoise.play()
self.lastDingNoise = rospy.Time.now()
degree = random.choice(self.angleList)
if rospy.Time.now() - self.lastJumpNoise > rospy.Duration(
2):
self.jumpNoise.close()
self.jumpNoise = pw.Wav(
path.join(self.sound_folder, "jomp.wav"))
self.turn_game(degree)
break
#If a wall is within 1.5 meter make you turn
if self.dist < self.wall_threshold and self.dist > 0:
degree = random.choice(self.angleList)
if rospy.Time.now() - self.lastJumpNoise > rospy.Duration(2):
self.jumpNoise.close()
self.jumpNoise = pw.Wav(
path.join(self.sound_folder, "jomp.wav"))
self.turn_game(degree)
break
r.sleep()
def straight(self, start_x, start_y):
self.x = None
self.y = None
self.yaw = None
r = rospy.Rate(10)
while not self.yaw: #Check to see if data is coming in
print "NONE"
r.sleep()
pass
while not self.x: #Check to see if data is coming in
print "None"
r.sleep()
pass
straightAngle = self.yaw #the angle you are facing
errorAngle = 15*math.pi/180 #threshold of the angle
loc_x = self.x #get you location after you turn
loc_y = self.y #get you location after you turn
while not rospy.is_shutdown():
#this make sure htat you stay in straight angle
if abs(angle_diff(self.yaw, straightAngle)) < errorAngle: #Makes sure that you are walking in a straigt line
if rospy.Time.now() - self.lastBeepNoise > rospy.Duration(1): #Plays the beep sound every second
self.beepNoise.close()
self.beepNoise = pw.Wav(path.join(self.sound_folder, "beep3.wav"))
self.beepNoise.play()
self.lastBeepNoise = rospy.Time.now()
x = self.x - loc_x
y = self.y - loc_y
dist = math.sqrt((x**2)+(y**2))
if dist >= self.travel_dist: #Check to see if you have the goal of 1.5 meter
self.dingNoise.play()
self.lastDingNoise = rospy.Time.now()
degree = random.choice(self.angleList) #Choose a random angle from the angleList
if rospy.Time.now() - self.lastJumpNoise > rospy.Duration(2):
self.jumpNoise.close()
self.jumpNoise = pw.Wav(path.join(self.sound_folder, "jomp.wav"))
self.turn_game(degree) #Enter in the turn game method
break
out_x = self.x - start_x
out_y = self.y - start_y
outside = math.sqrt((out_x**2)+(out_y**2))
#this check to if you out of the border
if(outside > self.border_radius):
self.outOfBound(start_x, start_y)
break
r.sleep()
def __init__(self):
rospy.init_node("grid_game_2")
rospy.Subscriber("/tango_pose", PoseStamped, self.process_pose)
rospy.Subscriber("/wall_dist", Float64, self.process_dist)
srv = Server(GridGame2Config, self.config_callback)
top = RosPack().get_path('mobility_games')
self.sound_folder = path.join(top, 'auditory/sound_files')
self.engine = pyttsx.init()
self.dingNoise = pw.Wav(path.join(self.sound_folder, "ding.wav"))
self.jumpNoise = pw.Wav(path.join(self.sound_folder, "jomp.wav"))
self.beepNoise = pw.Wav(path.join(self.sound_folder, "beep3.wav"))
self.lastDingNoise = rospy.Time.now()
self.lastJumpNoise = rospy.Time.now()
self.lastBeepNoise = rospy.Time.now()
self.angleList = [90, -90, 180]
self.traveled_distance = rospy.get_param('~traveled_distance', 2.0)
self.wall_threshold = rospy.get_param('~wall_threshold', 1.5)
self.hasSpoken = False
def run(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
if rospy.Time.now() - self.lastDingNoise > rospy.Duration(2):
self.dingNoise.close()
self.dingNoise = pw.Wav(
path.join(node.sound_folder, "ding.wav"))
self.straight()
r.sleep()
def __init__(self):
rospy.init_node("grid_game")
rospy.Subscriber("/tango_pose", PoseStamped,self.process_pose)
srv = Server(OriginalGridGameConfig, self.config_callback)
top = RosPack().get_path('mobility_games')
self.sound_folder = path.join(top, 'auditory/sound_files')
self.engine = pyttsx.init()
self.dingNoise = pw.Wav(path.join(self.sound_folder, "ding.wav"))
self.jumpNoise = pw.Wav(path.join(self.sound_folder, "jomp.wav"))
self.beepNoise = pw.Wav(path.join(self.sound_folder, "beep3.wav"))
self.beep2Noise = pw.Wav(path.join(self.sound_folder, "beep2.wav"))
self.lastDingNoise = rospy.Time.now()
self.lastJumpNoise = rospy.Time.now()
self.lastBeepNoise = rospy.Time.now()
self.lastBeep2Noise = rospy.Time.now()
self.angleList = [90,-90,180]
self.travel_dist = 2.0
self.border_radius = 5.0
self.hasSpoken = False
def __init__(self):
top = RosPack().get_path('mobility_games')
self.sound_folder = path.join(top, 'auditory/sound_files')
rospy.init_node('audio_feedback')
rospy.Subscriber('/tango_pose', PoseStamped, self.process_pose)
self.x = None # x and y position of Tango
self.y = None
self.yaw = None
self.start = False
self.music_to_play = "ambience2.wav"
self.mus = pw.Wav(path.join(self.sound_folder, self.music_to_play)) # Loads music from wav file
print("Initialization done.")
def outOfBound(self, start_x, start_y):
speak = "Out of Bounds"
self.engine.say(speak)
if not self.hasSpoken:
self.engine.runAndWait()
self.engine.say(speak)
self.engine.runAndWait()
self.hasSpoken = True
normal = [start_x - self.x, start_y - self.y] #normal vector to the start
theta = [0, 90, 180 , -90] #all the possible angle
best_angle = 0
best_dotprod = -1
for i in range(len(theta)): #this calculate al possible angle that make you go back inot the circle
goal_angle = self.yaw + theta[i]*math.pi/180
unit = [math.cos(goal_angle), math.sin(goal_angle)]
dotprod = normal[0] * unit[0] + normal[1] * unit[1]
if dotprod > best_dotprod:
best_dotprod = dotprod
best_angle = theta[i]
if rospy.Time.now() - self.lastJumpNoise > rospy.Duration(2):
self.jumpNoise.close()
self.jumpNoise = pw.Wav(path.join(self.sound_folder, "jomp.wav"))
self.turn_game(best_angle)
x_1 = self.x
y_1 = self.y
dist = 0
straightAngle = self.yaw
errorAngle = 15*math.pi/180
self.engine.say("Walk Straight")
while dist < 1: #this make you go back inot circle for a meter
if abs(angle_diff(self.yaw, straightAngle)) < errorAngle: #Makes sure that you are walking in a straigt line
if rospy.Time.now() - self.lastBeep2Noise > rospy.Duration(1): #Plays the beep sound every second
self.beep2Noise.close()
self.beep2Noise = pw.Wav(path.join(self.sound_folder, "beep2.wav"))
self.beep2Noise.play()
self.lastBeep2Noise = rospy.Time.now()
x_2 = self.x - x_1
y_2 = self.y - y_1
dist = math.sqrt((x_2**2)+(y_2**2))
def config_callback(self, config, level):
self.mode = config["mode"]
self.sweep_tolerance = config["sweep_tolerance"]
self.sweep_range = config["sweep_range"]
self.count_sweeps = config["count_sweeps"]
self.reward_sound_file = config["rewardSound"]
self.reward_sound_object = pw.Wav(self.reward_sound_file)
# set reward checkpoints
for num in [10, 20, 50, 100, 250, 500, 1000]:
self.reward_at[num] = config["reward_at_%s" % num]
return config
def run(self):
r = rospy.Rate(10)
self.x = None
while not self.x: #Check to see if data is coming in
print "None Run Method"
r.sleep()
pass
starting_pos_x = self.x #get you starting location
starting_pos_y = self.y #git you starting location
while not rospy.is_shutdown():
if rospy.Time.now() - self.lastDingNoise > rospy.Duration(2):
self.dingNoise.close()
self.dingNoise = pw.Wav(path.join(node.sound_folder, "ding.wav"))
self.straight(starting_pos_x, starting_pos_y)
r.sleep()
def run_wav(self):
""" Plays a wav file while the user is sweeping the cane consistently.
Pauses music when the cane stops, and resumes when it continues."""
r = rospy.Rate(10)
print("Searching for Tango...")
print("Searching for AR tags...")
visible_tag = 0
while not rospy.is_shutdown():
if not self.start:
print("X position: " + str(self.x),
"Cane x position: " + str(self.cane_x))
# Start program if receiving pose from Tango
if self.x and self.cane_x and not self.start:
# Note initial position
self.x_init = self.x
self.y_init = self.y
# Set some initial conditions
self.start = True
self.cane_y_prev = self.cane_y
last_sweep = rospy.Time.now()
tag = 0
# Keep track of which tag's audio should be playing and is
# currently playing. -1 means no tags are applicable.
should_play = -1
playing = -1
print("Connection established.")
print("Cane found.")
# Set parameters for cane sweeping.
sweep_width = self.sweep_range
cane_points = [-sweep_width / 2.0, sweep_width / 2.0]
offset_selection = 0
if self.start:
# Selects the first tag in view
if len(self.id_list):
if self.id_list[0] in self.tag_to_music_object:
visible_tag = self.id_list[0]
# Determine the next point the cane should reach
offset = cane_points[offset_selection]
# If the cane has passed the midline (y = 0), start music
if (self.cane_y - offset) * (self.cane_y_prev - offset) <= 0:
self.num_sweeps += 1
should_play = visible_tag
last_sweep = rospy.Time.now()
print("Reached point %s." % offset_selection)
offset_selection = (offset_selection +
1) % len(cane_points)
# If in sound mode, play corresponding sound
if self.mode == "sound":
if self.count_sweeps:
self.engine.say(str(self.num_sweeps))
if not self.hasSpoken:
self.engine.runAndWait()
self.engine.say(str(self.num_sweeps))
self.engine.runAndWait()
self.hasSpoken = True
else:
# play sound
self.tag_to_sound_object[should_play].close()
self.tag_to_sound_object[should_play] = \
pw.Wav(os.path.join(self.sound_folder,
self.tag_to_sound_file[should_play]))
self.tag_to_sound_object[should_play].play()
if self.num_sweeps in self.reward_at \
and self.reward_at[self.num_sweeps]:
if self.reward_sound_object:
self.reward_sound_object.close()
self.reward_sound_object = \
pw.Wav(self.reward_sound_file)
self.reward_sound_object.play()
# Note previous cane position
if self.cane_y_prev != self.cane_y:
self.cane_y_prev = self.cane_y
# If no sweep in X seconds, stop music
if rospy.Time.now() - last_sweep > rospy.Duration(
self.sweep_tolerance):
should_play = -1
print self.mode
# Music should not play if not in music mode
if self.mode != "music":
should_play = -1
# Play music if it should be playing
if playing != should_play and should_play >= 0:
self.tag_to_music_object[should_play].play()
playing = should_play
# Pause music if it should be paused
if should_play == -1 and playing >= 0:
self.tag_to_music_object[playing].pause()
playing = -1
# Stop playing music to tags other than current one
for tag in self.tag_to_music_object:
if self.tag_to_music_object[tag].stream.is_active() and \
tag != playing:
self.tag_to_music_object[tag].pause()
# Loop music if it has reached the end of the track
if playing >= 0 and not \
self.tag_to_music_object[playing].stream.is_active():
self.tag_to_music_object[playing].close()
self.tag_to_music_object[playing] = \
pw.Wav(os.path.join(self.sound_folder,
self.tag_to_music_file[visible_tag]))
self.tag_to_music_object[playing].play()
# Run at 10 loops per second
r.sleep()
def run(self):
freq = ac.freq('C5') #set default pitch to be overwritten and saved.
vol = .8 #set default volume to be overwritten and saved.
while not rospy.is_shutdown(): #Start main while loop
#self.dist = random.random()*4
if not self.dist is None and self.on: #check if is on and has a distance
self.last_sound_time = rospy.Time.now() #reset sound time
if abs(self.dist) < self.altsounddist and self.ding:
rewardsound = pw.Wav(self.dingsound)
rewardsound.play()
self.dingHappened = True
else:
freq = ac.freq(
'C5') #set default pitch (to be overwritten)
vol = .8 #set default volume (to be overwritten)
seconds = 5 * self.rate
fadeinendtime = 1.0 * self.rate
fadeoutendtime = 1.0 * self.rate
fadeintime = .2 * self.rate
fadeouttime = .2 * self.rate
""" MAKE THIS A PART OF THE UPDATE OF THE POSITION INSTEAD OF RECALCULATING ALL OF THIS FOR EVERY SOUND PLAYED!!!!!"""
if (self.pitch): #if pitch modulation
multiplier = self.maxfreq / math.pow(
2, (self.maxfreqdist - self.altsounddist)
) #calculate multiplier based on maxfreq, maxfreqdist, and altsounddist
freq = max(
min(
multiplier * math.pow(
(abs(self.dist + random.random() *
self.randrange) - self.altsounddist),
2) + 65.4 * 2, self.maxfreq),
65.4 * 2) * 2 #set frequency based on distance
freq = ac.quantize(freq,
quantizetype=self.quantizetype,
key=self.key) #quantize audio
else:
try: #try to set default pitch
freq = ac.freq(self.pitchdefault)
except: #if that doesn't work, do nothing, leaving the pitch as whatever it previously was.
pass
if (self.vol): #if volume modulation
vol = (
.8 / (1 + math.exp((self.dist) / 1.5 - 1)) + .2
) * vol #insert volume function here. that goes from 1 to 0 with increasing distance to wall.
else:
vol = self.voldefault
#self.synth = ac.delay(self.synth, 1, self.s*.2, .5) #add delay to the synth
if not self.track: #if no track
synth = ac.synth(
freq, synth="sin",
fadebool=False) #create synth for sound
synth = ac.fade_out(
ac.fade_in(synth, fadeinendtime, fadeintime),
fadeoutendtime, fadeouttime)
ac.playstream(
synth, self.player, seconds=seconds, volume=vol
) #synth, self.player, seconds = .5, volume = vol)
#print(walldistself.dist)
self.r.sleep() #wait until next iteration
if (self.dingHappened):
self.dingHappened = False
rewardsound.close()
def run(self):
lps = 10 # loops per second!
r = rospy.Rate(lps)
has_spoken = False
p = pyaudio.PyAudio # Establish pyaudio object
playing = False
print("Script starting.")
while not rospy.is_shutdown():
if self.x and not self.start:
self.start = True
jump_t = 0.5 # duration (s) jump measurements are sampled
dist_t = 0.5 # movement
rot_t = 1.0 # rotation
distance = [0] * int(dist_t * lps) # Create list of keyframes
rotation = [0] * int(rot_t * lps) # for movement, rotation, and
zd = [0] * int(jump_t * lps) # z position.
# These will be used to tell whether the user is moving, jumping, or rotating.
last_beep = rospy.Time.now()
beep_frequency = 1
last_whoosh = rospy.Time.now()
whoosh_frequency = 1
x_curr, y_curr, z_curr, yaw_curr = self.x, self.y, self.z, self.yaw
if self.x and self.start:
x_prev = x_curr
y_prev = y_curr
x_curr = self.x
y_curr = self.y
# Update position for this loop
change = math.sqrt((x_curr - x_prev)**2 + (y_curr - y_prev)**2)
distance = [change] + distance[:-1]
total_distance = sum(distance) # Finds horizontal distance since last frame
if total_distance > 0.5 and playing == False:
self.mus.play() # Play music if user has moved more than half a meter in last 500ms
playing = True
elif total_distance > 0.25:
pass
else:
# Stop music if user has moved less than a quarter meter in last 500ms
self.mus.pause()
playing = False
if not self.mus.stream.is_active() and playing:
self.mus.close()
self.mus = pw.Wav(path.join(self.sound_folder, self.music_to_play))
self.mus.play()
z_prev = z_curr
z_curr = self.z
change = (z_curr - z_prev)
zd = [change] + zd[:-1]
total_jump = sum(zd)
if total_jump > 0.2:
# If z position has increased by at least 0.2m, user has jumped.
system('aplay ' + path.join(self.sound_folder, "jomp.wav"))
zd = [0] * int(jump_t * lps)
yaw_prev = yaw_curr
yaw_curr = self.yaw
change = yaw_curr - yaw_prev
if abs(change) > math.pi:
change -= 2*math.pi*np.sign(change)
rotation = [change] + rotation[:-1]
total_rotation = sum(rotation)
if abs(total_rotation) > math.pi and rospy.Time.now() - last_whoosh > rospy.Duration(whoosh_frequency):
system('aplay ' + path.join(self.sound_folder, "ding.wav"))
rotation = [0] * int(rot_t * lps)
last_whoosh = rospy.Time.now()
print "Distance: " + str(round(total_distance, 3))
print "Rotation: " + str(round(total_rotation, 3))
r.sleep()
self.mus.close()
