def turn_game(self, angle):
self.yaw = None
sound = {90: "Turn Left", -90: "Turn right", 180: "Turn around"}
speech = sound[angle]
self.engine.say(speech)
if not self.hasSpoken:
self.engine.runAndWait()
self.engine.say(speech)
self.engine.runAndWait()
self.hasSpoken = True
r = rospy.Rate(10) #set the loop rate
while not self.yaw: #not reieveing data from streamer
print self.yaw
r.sleep()
pass
#calculate the targeted angle
goal_angle = self.yaw + angle * math.pi / 180
goal_angle %= 2 * math.pi #puts it in the form of 2pi
threshold = 20 * math.pi / 180 #angle thresh hold
while not rospy.is_shutdown():
#Once you are in the angle thresh hold play the sound
if abs(angle_diff(self.yaw, goal_angle)
) <= threshold: #Make sure if you hit the threshhold
self.jumpNoise.play()
self.lastJumpNoise = rospy.Time.now()
break
r.sleep()
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 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 turn_game(self,angle):
self.yaw = None
sound = {90:"Turn Left", -90:"Turn right", 180:"Turn around"}
speech = sound[angle]
self.engine.say(speech) #Says the turn accordng to the angle
if not self.hasSpoken: #Make sures that it doens't get interrupted
self.engine.runAndWait()
self.engine.say(speech)
self.engine.runAndWait()
self.hasSpoken = True
r = rospy.Rate(10)
while not self.yaw: #Let us if data is coming in
print self.yaw
r.sleep()
pass
goal_angle = self.yaw + angle*math.pi/180
goal_angle %= 2*math.pi
threshold = 20*math.pi/180
while not rospy.is_shutdown():
if abs(angle_diff(self.yaw, goal_angle)) <= threshold: #Make sure if you hit the threshhold
self.jumpNoise.play()
self.lastJumpNoise = rospy.Time.now()
break #Break from the loop
r.sleep()