action1 = 6
if action > 0:
timestemp += 1
if not bot.action(action0, action1):
continue
scan = bot.scan()
pose = bot.pose
traj_truth.append(pose)
neff = pf.feed(action0, action1, scan)
bid = pf.bestSampleId()
bp = pf.getParticle(bid)
gain = Viewer.SensorMapping(grid, pose, scan['data'], pyparam)
print(gain)
mmmm = grid.getObserv((pose[0], pose[1]), pose[2], 32, 32)
mmmm = Viewer.Map2Image(mmmm)
cv2.imshow('mmmm', mmmm)
img = Viewer.DrawEnv(im, 1.0, pose, scan['data'], pyparam)
for i in range(pf_size):
if i != bid:
traj = pf.getParticle(i).getTraj()
#print(traj)
img = Viewer.DrawTraj(img, 1, traj, (0, 255, 0))
img = Viewer.DrawTraj(img, 1, bp.getTraj(), (0, 255, 0))
img = Viewer.DrawTraj(img, 1, traj_truth, (255, 0, 0))
cv2.imshow('env', img)
mm = grid.getWholeMapProb()
mm = Viewer.Map2Image(mm)
mm2 = bp.getMap().getWholeMapProb()
action = 2
if k==ord('a'):
ws.send('left')
action = 3
if k==ord('d'):
ws.send('right')
action = 4
data_ready = False
if action > 0:
time.sleep(0.1)
print(ws.recv())
ws.send("is collided?")
print(ws.recv())
reading_temp = [scale*i for i in reading]
scan = {'sensor_size': 61,
'start_angle': -30.0,
'end_angle': 210.0,
'max_dist': max_dist,
'data': reading_temp}
#print(reading_temp)
neff = pf.feed(action, scan)
bid = pf.bestSampleId()
bp = pf.getParticle(bid)
mm2 = bp.getMap().getWholeMapProb()
mm2 = Viewer.Map2Image(mm2)
cv2.imshow('map_p', mm2)
if neff < 0.5:
pf.resampling()