-
Notifications
You must be signed in to change notification settings - Fork 0
/
simulate_py.py
64 lines (51 loc) · 1.21 KB
/
simulate_py.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
# -*- coding: utf-8 -*-
"""simulate.py
Automatically generated by Colaboratory.
Original file is located at
https://colab.research.google.com/drive/1wt7KgOIlLQ_NbQ6a70Lf17Y2C7stpE-N
"""
import localizer
import random
from copy import deepcopy
from matplotlib import pyplot as plt
class Simulation(object):
def __init__(self, grid, blur, p_hit,start_pos=None):
self.grid = grid
self.beliefs = localizer.initialize_beliefs(self.grid)
def prepare_visualizer(self):
self.X = []
self.Y = []
self.P = []
def show_beliefs(self,past_turn=False):
if past_turn:
X = deepcopy(self.X)
Y = deepcopy(self.Y)
P = deepcopy(self.P)
del(self.X[:])
del(self.Y[:])
del(self.P[:])
R = 'r'
G = 'g'
blur = 0.05
p_hit = 200.0
grid = [
[R,G,G,G,R],
[G,G,R,G,R],
[G,R,G,G,G],
[R,R,G,R,G],
[R,G,R,G,R],
]
localizer.initialize_beliefs(grid)
true_pose = None
X = []
Y = []
P = []
for y, row in enumerate(localizer.initialize_beliefs(grid)):
for x, belief in enumerate(row):
X.append(x)
Y.append(5-y-1)
P.append(5000.0 * belief)
plt.figure()
plt.scatter(X,Y,s=P,color="blue")
plt.scatter([true_pose[1]], [height-true_pose[0]-1], color='red', marker="*", s=200)
plt.show()