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