def generateMap():
canvas = pds.Canvas()
map = pds.Map()
# Definitions of walls
# a: O to A
# b: A to B
# c: C to D
# d: D to E
# e: E to F
# f: F to G
# g: G to H
# h: H to O
map.add_wall((0, 0, 0, 168))
# a
map.add_wall((0, 168, 84, 168))
# b
map.add_wall((84, 126, 84, 210))
# c
map.add_wall((84, 210, 168, 210))
# d
map.add_wall((168, 210, 168, 84))
# e
map.add_wall((168, 84, 210, 84))
# f
map.add_wall((210, 84, 210, 0))
# g
map.add_wall((210, 0, 0, 0))
# h
map.draw()
return map
estimate_x
] * NUMBER_OF_PARTICLES # At the start every particle has its intial coords as 0,0,0
p_y = [estimate_y] * NUMBER_OF_PARTICLES
p_theta = [estimate_theta] * NUMBER_OF_PARTICLES
weights = [
1.0 / NUMBER_OF_PARTICLES
] * NUMBER_OF_PARTICLES # In an actual example these would be updated
mu = 0 # update with actual value
# sigma values for 20cm dist and pi/2 rotations
sigma_e = 0.1
sigma_f = 0.04519
sigma_g = 0.03490
# from particle Data Structures
mymap = world.Map()
particles = world.Particles() # for printing
for i in range(100):
particles.data.append((estimate_x, estimate_y, estimate_theta, weights[0]))
world.init_world(mymap)
# estimate L01 position based on particles
def update_estimate():
global p_x
global p_y
global p_theta
global weights
estimate_x = 0.0
estimate_y = 0.0
(reading, _) = interface.getSensorValue(3)
reading = reading - 12
return (getNewX(x, e, theta,
distance), getNewY(y, e, theta,
distance), getNewThetaStraight(theta),
calculate_likelihood(x, y, theta, w, reading))
def getNextParticleRotation((x, y, theta, w), alpha):
(reading, _) = interface.getSensorValue(3)
reading = reading - 12
return (x, y, getNewThetaRotation(theta, alpha),
calculate_likelihood(x, y, theta, w, reading))
mymap = particleDataStructures.Map()
mymap.add_wall((0, 0, 0, 168))
#
mymap.add_wall((0, 168, 84, 168))
#
mymap.add_wall((84, 126, 84, 210))
#
mymap.add_wall((84, 210, 168, 210))
#
mymap.add_wall((168, 210, 168, 84))
#