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]))