/
Map.py
152 lines (140 loc) · 5.16 KB
/
Map.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
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
class Map2D(object):
"""Store the position of obstacles in a grid.
Right now it can only load from a single file.
The list of all obstacles is stored in self.obstacles
and a grid with the obstacles near each part
of the space is stored in self.grid.
The obstacles are input through *data*.
This can be either a list of points or
a filename / file-object with the data in it.
"""
def __init__(self, data, radius, x0=None, xf=None, y0=None, yf=None):
self.minLx = x0
self.maxLx = xf
self.minLy = y0
self.maxLy = yf
self.obstacles = []
self.grid_updated = False
self.load_obstacles(data)
nx = int(self.Lx / radius)
ny = int(self.Ly / radius)
self.setup_boxes(nx, ny)
self.fill_grid()
return
def load_obstacles(self, obstacles):
if type(obstacles) == str:
self.add_from_name(obstacles)
elif type(obstacles) == file:
self.add_from_file(obstacles)
elif type(obstacles) == list:
self.add_from_list(obstacles)
else:
raise Exception("Map2D: unknown data type {:}".format(obstacles))
return
def add_from_name(self, filename):
"""Read a collection of *x y* pairs
from the file *filename*.
The dimensions of the available space
are deduced from these points.
"""
with open(filename, 'r') as f:
self.add_from_file(f)
return
def add_from_file(self, f):
"""Read a collection of *x y* pairs
from the file object *f*.
The dimensions of the available space
are deduced from these points.
"""
obstacles = []
while True:
try:
x, y = [float(o) for o in f.readline().split()]
obstacles.append([x, y])
except:
break
self.add_from_list(obstacles)
return
def add_from_list(self, obstacles):
"""Load the obstacles from *obstacles*."""
xs = ([o[0] for o in obstacles])
ys = ([o[1] for o in obstacles])
# Determine the limits of the box
if self.minLx is None:
self.minLx = min(xs) - 0.01 * abs(min(xs))
if self.maxLx is None:
self.maxLx = max(xs) + 0.01 * abs(max(xs))
self.Lx = self.maxLx - self.minLx
if self.minLy is None:
self.minLy = min(ys) - 0.01 * abs(min(ys))
if self.maxLy is None:
self.maxLy = max(ys) + 0.01 * abs(max(ys))
self.Ly = self.maxLy - self.minLy
self.obstacles.extend(obstacles)
self.grid_updated = False
return
def setup_boxes(self, nx, ny):
""" Define how many boxes per axis the grid will have.
Total number of boxes = nx * ny.
nx = number of boxes along x. (int > 3)
ny = number of boxes along y. (int > 3)
"""
self.nx = int(nx)
self.ny = int(ny)
self.grid = [[] for i in range(nx * ny)] # initialize empty grid
return
def which_box(self, pos):
""" Gives the (i,j) indexes corresponding to position pos.
"""
i = int((pos[0] - self.minLx) * self.nx / self.Lx)
j = int((pos[1] - self.minLy) * self.ny / self.Ly)
assert i >= 0 and i < self.nx
assert j >= 0 and j < self.ny
return (i, j)
def obstacles_in_box(self, i, j):
"""Returns a list with the agents in a given box.
Periodic boundaries implemented, so if i (j) is larger than
nx (ny) it is replaced by i%nx (j%ny).
i = inner-most index of the box. (int)
j = outer-most index of the box. (int)
"""
i, j = i % self.nx, j % self.ny
return self.grid[i + j * self.nx]
def obstacles_near(self, pos):
i, j = self.which_box(pos)
return self.obstacles_in_box(i, j)
def fill_grid(self):
""" Fill the grid[] with a list of the obstacles contained
in each element. If obstacle is stored in its box + the 8
surrounding ones, so that a robot that senses obstacles in
its box will have information of its surrounding.
"""
nx, ny = self.nx, self.ny
for i in range(nx * ny):
self.grid[i] = []
for o in self.obstacles:
i, j = self.which_box(o)
for dj in (-1, 0, 1):
jj = j + dj
if jj >= 0 and jj < ny:
for di in (-1, 0, 1):
ii = i + di
if ii >= 0 and ii < nx:
self.grid[ii + jj * nx].append(o)
self.grid_updated = True
return self.grid
def filtered_map(self, threshold=1):
"""For each grid box that contains
at least *treshold* obstacles,
return their mean position.
"""
fmap = []
if not self.grid_updated:
self.fill_grid()
for i in range(self.nx * self.ny):
obs = self.grid[i]
if len(obs) >= threshold:
x = sum(o[0] for o in obs) / len(obs)
y = sum(o[1] for o in obs) / len(obs)
fmap.append([x, y])
return fmap