def sample_resample(particles, resample_num, landmark_location, measured_angle, measured_distance):
print(landmark_location)
# Adding uncertainty to the particles 10
# particle.add_uncertainty_von_mises(particles, 0, 0.02)
particle.add_uncertainty(particles, 10, 0.08)
# Calculating weights for angles
particleAngles = [particle.getTheta() for particle in particles]
e_l = [((landmark_location[0] - particle.getX()) , (landmark_location[1] - particle.getY())) / np.linalg.norm([(particle.getX() - landmark_location[0]), (particle.getY() - landmark_location[1])]) for particle in particles]
e_theta = [np.array((np.cos(angle),np.sin(angle))) for angle in particleAngles]
phi = [np.arccos(np.dot(x,y)) for x, y in zip(e_l, e_theta)]
e_hat_theta = [(-np.sin(angle),np.cos(angle)) for angle in particleAngles]
particle_angle_diff_temp = [np.dot(x,y) for x,y in zip(e_l,e_hat_theta)]
particle_angle_diff = [np.sign(x) * y for x,y in zip(particle_angle_diff_temp, phi)]
angleWeights = calcWeights(particle_angle_diff, measured_angle, 0.08)
#print ("angle weights after: {}".format(angleWeights))
# Weights for distance
particleLengths = [np.linalg.norm([(particle.getX() - landmark_location[0]), (particle.getY() - landmark_location[1])]) for particle in particles]
# Var should be roughly 50
lengthWeights = calcWeights(particleLengths, measured_distance, 20)
#print ("length weights after: {}".format(lengthWeights))
# Combining the weight types
'''
normWeights = []
for i in range(resample_num):
normWeights.append(angleWeights[i] * lengthWeights[i])
'''
normWeights = [x * y for x, y in zip(angleWeights, lengthWeights)]
normWeights = normWeights/np.sum(normWeights)
# Setting the weight for each particle
for i in range(resample_num):
particles[i].setWeight(normWeights[i])
# Resampling
# Chooses resample_num random particles from the current particles based on the calculated weights.
temp = np.random.choice(particles, resample_num, True, normWeights)
retVal = []
for elem in temp:
retVal.append(copy.deepcopy(elem))
return retVal
def move_robot(oneMeter, particles, direction, amount):
distNoise = 0.1
driveAngleNoise = 0.0002 * amount
turnAngleNoise = 0.001 * amount
if (direction == 'r' or direction == 'l') and amount < 8:
amount += 5
if direction == 'f':
remaining, stop_dir = mvmt.goForward(amount/100.0, oneMeter)
particle.add_uncertainty(particles, (amount - (remaining*100))*distNoise, driveAngleNoise)
move_particles(particles, (amount - (remaining*100)), 0)
return remaining, stop_dir, particles
elif direction == 'b':
remaining, stop_dir = mvmt.goBack(amount/100.0, oneMeter)
particle.add_uncertainty(particles, (amount - (remaining*100))*distNoise, driveAngleNoise)
move_particles(particles, -(amount - (remaining*100)), 0)
return remaining, stop_dir, particles
elif direction == 'r':
mvmt.goRight(amount)
particle.add_uncertainty(particles, 5, turnAngleNoise)
move_particles(particles, 0, amount)
return 0,'n', particles
elif direction == 'l':
mvmt.goLeft(amount)
particle.add_uncertainty(particles, 5, turnAngleNoise)
move_particles(particles, 0, -amount)
return 0,'n', particles
else:
print("Not a valid direction")
return -1, 'n', particles
def move_robot(particles, direction, amount):
distNoise = 0.1 * amount
driveAngleNoise = 0.0002 * amount
turnAngleNoise = 0.001 * amount
if direction == 'f':
remaining, stopDir = mvmt.goForward(amount / 100.0)
particle.add_uncertainty(particles, distNoise, driveAngleNoise)
move_particles(particles, (amount - remaining), 0)
return remaining, stopDir
elif direction == 'b':
remaining, stopDir = mvmt.goBack(amount / 100.0)
particle.add_uncertainty(particles, distNoise, driveAngleNoise)
move_particles(particles, -(amount - remaining), 0)
return remaining, stopDir
elif direction == 'r':
mvmt.goRight(amount)
particle.add_uncertainty(particles, 0, turnAngleNoise)
move_particles(particles, 0, -amount)
return 0, 'n'
elif direction == 'l':
mvmt.goLeft(amount)
particle.add_uncertainty(particles, 0, turnAngleNoise)
move_particles(particles, 0, amount)
return 0, 'n'
else:
print("Not a valid direction")
return -1, 'n'
def stop(wait):
global nextmove
global currentmove
global est_pose
# send stop command to the adrino
# calculate and apply the movemt to the particles
msg = 's'
serialRead.write(msg.encode('ascii'))
runtime = time.time() - movestarttime
waittime = time.time() + wait
if runtime > 1.0:
if currentmove == "F":
if runtime < 1.2:
distance = 0
else:
distance = forward_time_2_cm(runtime)
for p in particles:
x = p.getX() + (np.cos(p.getTheta()) * distance)
y = p.getY() + ((-np.sin(p.getTheta())) * distance)
p.setX(x)
p.setY(y)
add_uncertainty(particles, 5, 0.2)
elif currentmove == "H":
distance = cw_time_2_deg(runtime) * (np.pi / 180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta - distance)
add_uncertainty(particles, 1, 0.2)
elif currentmove == "L":
distance = ccw_time_2_deg(runtime) * (np.pi / 180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta + distance)
add_uncertainty(particles, 1, 0.2)
currentmove = "0"
est_pose = estimate_pose(particles)
def stop(wait):
global nextmove
global currentmove
global est_pose
# send stop command to the adrino
# calculate and apply the movemt to the particles
msg = 's'
serialRead.write(msg.encode('ascii'))
runtime = time.time() - movestarttime
waittime = time.time() + wait
if runtime > 1.0:
if currentmove == "F":
if runtime < 1.2:
distance = 0
else:
distance = forward_time_2_cm(runtime)
for p in particles:
x = p.getX() + (np.cos(p.getTheta()) * distance)
y = p.getY() + ((-np.sin(p.getTheta())) * distance)
p.setX(x)
p.setY(y)
add_uncertainty(particles, 5, 0.2)
elif currentmove == "H":
distance = cw_time_2_deg(runtime) * (np.pi/180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta - distance)
add_uncertainty(particles, 1, 0.2)
elif currentmove == "L":
distance = ccw_time_2_deg(runtime) * (np.pi/180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta + distance)
add_uncertainty(particles, 1, 0.2)
currentmove = "0"
est_pose = estimate_pose(particles)
if sample >= cumsum[i] and sample < cumsum[i + 1]:
samples.append(copy.deepcopy(particles[i]))
for i in range(len(particles)):
particles[i] = samples[i]
# Draw detected pattern
cam.draw_object(colour)
else:
LMInSight = False
# No observation - reset weights to uniform distribution
for p in particles:
p.setWeight(1.0 / num_particles)
par.add_uncertainty(particles, 5, 0.15)
est_pose = par.estimate_pose(
particles) # The estimate of the robots current pose
print "In sight = {0}".format(LMInSight)
print "Mean weight = {0}".format(weightMean)
print "Visited lms: ", visitedLM
# XXX: Make the robot drive
if weightMean > 0.6 and LMInSight and not visitedLM[lastSeenLM]:
turn_counter = 0
# Turn towards landmark
if lastMeasuredAngle > 0:
arlo.go_diff(30, 29, 0, 1)
elif lastMeasuredAngle < 0:
def drive_to_middle(particles, pose, landmarks_seen):
dest = find_vec_to_mid(pose)
pos_vec = (np.cos(pose.getTheta()),np.sin(pose.getTheta()))
angle_to_turn, dist_to_drive, _ = get_directions(pos_vec, dest)
#print("Drive?:", (len(landmarks_seen) > 1), (abs(angle_to_turn) < 0.18))
#print("Angle to turn:", angle_to_turn)
#print("Length to drive:", dist_to_drive)
dist_thresh = 10
angle_thresh = 0.15
stdX = [elem.getX() for elem in particles]
stdY = [elem.getY() for elem in particles]
converged = np.std(stdX) < 80 and np.std(stdY) < 80
if converged and len(landmarks_seen) > 1:
if abs(angle_to_turn) < angle_thresh and dist_to_drive > dist_thresh:
print("Driving")
move_robot(particles, 'f', dist_to_drive)
elif angle_to_turn < 0 and abs(angle_to_turn) > angle_thresh and dist_to_drive > dist_thresh:
print("Turning Left")
move_robot(particles, 'l', -np.degrees(angle_to_turn))
elif angle_to_turn > 0 and abs(angle_to_turn) > angle_thresh and dist_to_drive > dist_thresh:
print("Turning Right")
move_robot(particles, 'r', np.degrees(angle_to_turn))
if dist_to_drive <= dist_thresh:
print("Finding Green Box")
pos_vec = (np.cos(pose.getTheta()),np.sin(pose.getTheta()))
angle_to_turn, green_dist, _ = get_directions(pos_vec, (300,0))
if abs(angle_to_turn) > angle_thresh:
if angle_to_turn < 0:
print("Turning Left to Green Box")
move_robot(particles, 'l', -np.degrees(angle_to_turn))
return False, 5
elif angle_to_turn > 0:
print("Turning Right to Green Box")
move_robot(particles, 'r', np.degrees(angle_to_turn))
return False, 5
else:
print("Finished")
move_robot(particles, 'r', 720)
return True, 1
else:
return False, 5
elif converged:
print("Something went wrong")
landmarks_seen = []
particle.add_uncertainty(particles, 200, 1)
return False, 5
elif len(landmarks_seen) > 1:
print("Not converged, but all landmarks seen")
if angle_to_turn < 0:
move_robot(particles, 'l', -np.degrees(angle_to_turn))
return False, 5
elif angle_to_turn > 0:
move_robot(particles, 'r', np.degrees(angle_to_turn))
return False, 5
else:
print("Finding landmarks")
move_robot(particles, 'l', 30)
return False, 5
former_turn += updated_values[2]
angular_velocity = updated_values[1]
velocity = updated_values[0]
num_particles = len(particles)
for p in particles:
# calculates new orientation
if angular_velocity != 0:
curr_angle = add_to_angular(p.getTheta(), angular_velocity)
if velocity != 0:
[x, y] = move_vector(p, velocity)
particle.move_particle(p, x, y, curr_angle)
if velocity != 0:
particle.add_uncertainty(particles, 12, 15)
if velocity == 0 and angular_velocity != 0:
particle.add_uncertainty(particles, 0, 15)
# Fetch next frame
colour, distorted = cam.get_colour()
# Detect objects
objectType, measured_distance, measured_angle, colourProb = cam.get_object(
colour)
if objectType != 'none':
obs_landmark = ret_landmark(colourProb, objectType)
print obs_landmark
sum_of_angle_diff = 0.0
list_of_particles = weight_particles(particles,
np.degrees(measured_angle),
def update_particles(particles, cam, velocity, angular_velocity, world,
WIN_RF1, WIN_World):
#print 'update: ' + str(angular_velocity)
cv2.waitKey(4)
num_particles = len(particles)
for p in particles:
# calculates new orientation
curr_angle = add_to_angular_v2(np.degrees(p.getTheta()),
angular_velocity)
if velocity > 0.0:
[x, y] = move_vector(p, velocity)
particle.move_particle(p, x, y, curr_angle)
else:
particle.move_particle(p, 0.0, 0.0, curr_angle)
if velocity != 0.0:
particle.add_uncertainty(particles, 12, 5)
if velocity == 0.0 and angular_velocity != 0.0:
particle.add_uncertainty(particles, 0, 5)
# Fetch next frame
colour, distorted = cam.get_colour()
# Detect objects
objectType, measured_distance, measured_angle, colourProb = cam.get_object(
colour)
if objectType != 'none':
obs_landmark = ret_landmark(colourProb, objectType)
observed_obj = [
objectType, measured_distance, measured_angle, obs_landmark
]
list_of_particles = weight_particles(particles,
np.degrees(measured_angle),
measured_distance, obs_landmark)
particles = resample_particles(list_of_particles[:, [0, 2]])
particle.add_uncertainty(particles, 15, 5)
cam.draw_object(colour)
else:
observed_obj = [None, None, None, None]
for p in particles:
p.setWeight(1.0 / num_particles)
particle.add_uncertainty(particles, 10, 5)
est_pose = particle.estimate_pose(particles)
draw_world(est_pose, particles, world)
#Show frame
cv2.imshow(WIN_RF1, colour)
#Show world
cv2.imshow(WIN_World, world)
return {
'est_pos': est_pose,
'obs_obj': observed_obj,
'particles': particles
}
def update_particles(particles, cam, velocity, angular_velocity, world,
WIN_RF1, WIN_World):
raw_input()
print 'update: ' + str(angular_velocity)
cv2.waitKey(4)
num_particles = len(particles)
for p in particles:
# calculates new orientation
curr_angle = add_to_angular_v2(np.degrees(p.getTheta()), angular_velocity)
print 'theta_rad: ' + str(p.getTheta())
print 'theta_deg: ' + str(np.degrees(p.getTheta()))
print 'cur_ang_deg: ' + str(np.degrees(curr_angle))
if velocity > 0.0:
[x, y] = move_vector(p, velocity)
particle.move_particle(p, x, y, curr_angle)
else:
particle.move_particle(p, 0.0, 0.0, curr_angle)
print 'cur_ang_rad: ' + str(curr_angle)
if velocity != 0.0:
particle.add_uncertainty(particles, 12, 15)
if velocity == 0.0 and angular_velocity != 0.0:
particle.add_uncertainty(particles, 0, 15)
# Fetch next frame
colour, distorted = cam.get_colour()
# Detect objects
objectType, measured_distance, measured_angle, colourProb = cam.get_object(
colour)
if objectType != 'none':
obs_landmark = ret_landmark(colourProb, objectType)
observed_obj = [objectType, measured_distance, measured_angle,
obs_landmark]
list_of_particles = weight_particles(particles,
np.degrees(measured_angle),
measured_distance, obs_landmark)
particles = []
for count in range(0, num_particles):
rando = np.random.uniform(0.0,1.0) # np.random.normal(0.0, 1.0, 1)
# dicto = {'i': 500,
# 'n': 2}
p = when_in_range(list_of_particles,
0,
num_particles,
rando)
particles.append(
particle.Particle(p.getX(), p.getY(), p.getTheta(),
1.0 / num_particles))
print 'list_of_particles: ' + str(list_of_particles)
print 'particles: ' + str(particles)
particle.add_uncertainty(particles, 5, 2)
# new random particles added
#for c in range(0, int(math.ceil(num_particles * 0.05))):
# p = particle.Particle(500.0 * np.random.ranf() - 100,
# 500.0 * np.random.ranf() - 100,
# 2.0 * np.pi * np.random.ranf() - np.pi, 0.0)
# particles.append(p)
# Draw detected pattern
cam.draw_object(colour)
else:
observed_obj = [None, None, None, None]
# No observation - reset weights to uniform distribution
for p in particles:
p.setWeight(1.0 / num_particles)
particle.add_uncertainty(particles, 5, 2)
# est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
# return [est_pose, observed_obj]
est_pose = particle.estimate_pose(
particles) # The estimate of the robots current pose
print 'Updated pose: ' + str([est_pose.getX(), est_pose.getY()])
draw_world(est_pose, particles, world)
# Show frame
cv2.imshow(WIN_RF1, colour)
# Show world
cv2.imshow(WIN_World, world)
return {'est_pos': est_pose,
'obs_obj': observed_obj,
'particles': particles}
# # 'n': 2}
# p = when_in_range(list_of_particles,
# 0,
# num_particles,
# rando)
# # rando,
# # {'i': num_particles/2, 'n': 2},
# # num_particles)
# #weight_sum += p.getWeight()
# particles.append(particle.Particle(p.getX(), p.getY(), p.getTheta(), 1.0/num_particles))
print "resampled"
particle.add_uncertainty(particles, 15, 10)
# 10% new random particles added
# for c in range(0,int(math.ceil(num_particles*0.05))):
# p = particle.Particle(500.0*np.random.ranf() - 100, 500.0*np.random.ranf() - 100, 2.0*np.pi*np.random.ranf()-np.pi, 0.0)
# particles.append(p)
# Draw detected pattern
cam.draw_object(colour)
else:
# No observation - reset weights to uniform distribution
for p in particles:
p.setWeight(1.0/num_particles)
LMInSight = True
lastMeasuredAngle = measured_angle
weightMean = sampleWeights()
# Draw detected pattern
cam.draw_object(colour)
else:
LMInSight = False
# No observation - reset weights to uniform distribution
for p in particles:
p.setWeight(1.0 / num_particles)
par.add_uncertainty(particles, 2.5, 0.10)
# The estimate of the robots current pose
est_pose = par.estimate_pose(particles)
print "In sight = {0}".format(LMInSight)
print "Mean weight = {0}".format(weightMean)
print "Visited lms: ", visitedLM
# XXX: Make the robot drive
if avoiding_box:
avoid_box()
elif LMInSight and weightMean < 0.6:
print 'waiting for the mean to increase'
elif weightMean > 0.6 and LMInSight: