def sim_step(self):
        prev = self.loc
        nextmv = self.move_list.next()
        next = nextmv[0]
        angle = nextmv[1]
       
        move_vector = self.math.get_move_vector(prev, next)
        if move_vector is not(0,0):
            angle_noise = self.math.get_noise(0, self.ang_noise)
            #angle_noise = 0
            endpt = self.math.rotate_point(prev, move_vector, angle_noise)
            n_end = self.math.apply_point_noise(endpt.x, endpt.y, self.loc_noise, self.loc_noise, pret=True)

            ###### MAY CAUSE ERRORS #######

            # might be good to set this so that a value is added or
            # subtracted from this, so that the uncertainty of your
            # current bearing are taken into account.
            # subtracting this value from 315 gives an approximate of 0 to north
            #print self.initial_angle, 'SONARb', angle
            #self.initial_angle = 315 - printangle + angle_noise
            #self.initial_angle = angle + angle_noise
            #print self.initial_angle, 'SONARa', angle
            #self.initial_angle = 0
            
            ###### MAY CAUSE ERRORS #######

            self.loc = n_end
            self.set_heading(angle + angle_noise)
        #else:
        #    self.set_heading(angle)
        self.get_ranges()
        #self.ranges = [r/100 if r != -1 else r for r in self.ranges]
        self.out.publish(prev=point(prev.x, prev.y), next=point(next.x, next.y), angle=angle, ranges=self.ranges, actual=point(self.loc.x, self.loc.y), scan=self.scan_lines, head_line=self.head_line)
 def make_line(self, p1, p2, tp='lnstr'):
     """Make a line between the two points."""
     if tp == 'lnstr':
         try:
             return LineString([(p1.x, p1.y),(p2.x, p2.y)])
         except AttributeError:
             return LineString([(p1[0], p1[1]),(p2[0], p2[1])])
     elif tp == 'ros':
         try:
             return line(point(p1.x, p1.y), point(p2.x, p2.y))
         except AttributeError:
             return line(point(p1[0], p1[1]), point(p2[0], p2[1]))
    def run(self):
        data = proc_sonar()
        for i, p in enumerate(self.mv):
            fname = '%s/prc_%d_%d_%d.txt'%(self.fileloc, p[0],p[1], self.rng)
            try:
                f = open(fname, 'r')
                # print '%s/prc_%d_%d_%d.txt'%(self.fileloc, p[0],p[1], self.rng)
                data.prev = point(self.mv[i - 1][0],self.mv[i - 1][1]) if i is not 0 else point(self.mv[i][0], self.mv[i][1])
                data.next = point(self.mv[i][0], self.mv[i][1])
                data.angle = 0
                data.ranges = map(float, f.read().split(' '))
                self.pub.publish(data)
            except IOError:
                print 'file %s does not exist'%(fname)

            rospy.sleep(self.sleeptime) # artificial time an action takes
    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)
 def convert_line(self, ln):
     c = ln.coords
     return line(point(c[0][0], c[0][1]), point(c[1][0], c[1][1]))