def sim_step(self):
"""Moves the simulation into the next step."""
next = self.move_list.next()
current = self.loc if self.first else self.move_list.get_previous()[0] # THIS IS PROBABLY NOT THE ACTUAL LOCATION OF THE SONAR - noise has been added!
if next is -1:
return -1 # no more steps in list
else:
self.generate_particles(self.num_particles) # only if not already done
self.particles.resample() # only if particles exist and have weights
move_vector = self.math.get_move_vector(current, next[0])
self.move_to_noisy(move_vector, next[1]) # move sonar to its next position
self.get_ranges() # get sonar ranges
self.math.apply_range_noise(self.ranges, self.rng_noise) # apply noise to the sonar ranges
# get the vector required to move from the sonar's current
# point to the next point
for particle in self.particles.list():
# move each particle along the vector, and set its
# scan start angle tothe sonar's. This method
# introduces noise into both of those measurements.
particle.move(move_vector, self.initial_angle)
# get the ranges that the sonar would see if it were
# in the same position and orientation as the sonar
particle.get_ranges(self.scale)
# weight each particle based on the variation of its
# range measurements with the measurements received
# from the sonar.
self.weight_particle(particle)
print self.get_localisation_error()
self.save_info()
return 1 # steps remain in list
def update(self, data):
# maybe there should be separate methods which update based on
# action and sonar range data. This means that the localiser
# should be able to cope with backlogs in the actions since it
# will scan through the actions missed and move particles
# along all vectors that have not been processed yet.
to_move = data.next
angle = data.angle
prev_pos = data.prev
sonar_ranges = data.ranges
self.generate_particles(self.num_particles) # only if no particles are present in the list
self.particles.resample() # only if particles exist and have weights
move_vector = self.math.get_move_vector(prev_pos, to_move)
for particle in self.particles.list():
particle.move(move_vector, angle)
particle.get_ranges(self.scale)
self.weight_particle(particle)
def update(self, data):
# maybe there should be separate methods which update based on
# action and sonar range data. This means that the localiser
# should be able to cope with backlogs in the actions since it
# will scan through the actions missed and move particles
# along all vectors that have not been processed yet.
#if self.use_gui:
#self.updatepub.publish('update started')
to_move = data.next
angle = data.angle
prev_pos = data.prev
sonar_ranges = data.ranges
actual = data.actual
self.generate_particles(prev_pos, angle) # only if no particles are present in the list
self.particles.resample_meanvar()
#self.particles.resample() # only if particles exist and have weights
#for particle in self.particles.list():
# self.guipub.publish(weight=0.2, loc=point(particle.loc.x, particle.loc.y), angle=particle.initial_angle)
move_vector = self.math.get_move_vector(prev_pos, to_move)
#print '-------------'
for particle in self.particles.list():
particle.move(move_vector, angle)
particle.get_ranges(self.scale)
#print particle.ranges
self.weight_particle(sonar_ranges, angle, particle)
if self.use_gui:
mline = particle.move_line.coords
mv = line(point(mline[0][0], mline[0][1]), point(mline[1][0], mline[1][1]))
self.guipub.publish(weight=particle.wt, loc=point(particle.loc.x, particle.loc.y), angle=particle.heading, ranges=particle.ranges, moveline=mv, scan=particle.scan)
#print self.particles.mean
self.guipub.publish(weight=0.2, loc=point(self.particles.mean[0], self.particles.mean[1]), flag=1)
best = self.particles.best()
mline = best.move_line.coords
mv = line(point(mline[0][0], mline[0][1]), point(mline[1][0], mline[1][1]))
self.bestpub.publish(weight=best.wt, loc=point(best.loc.x, best.loc.y), angle=best.heading, ranges=best.ranges, moveline=mv, scan=best.scan, flag=2)
