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)