· 4 years ago · May 29, 2021, 09:42 AM
1"""OT15 - Exploration."""
2import PiBot
3import math
4
5
6class Robot:
7 """The robot class."""
8
9 def __init__(self):
10 """Class constructor."""
11 self.robot = PiBot.PiBot()
12 self.shutdown = False
13 self.sensed = False
14
15 self.l_enc = 0
16 self.r_enc = 0
17 self.last_l_enc = 0
18 self.last_r_enc = 0
19 self.front_middle_laser = 0
20
21 self.map = [["?", "?", "?"], ["?", " ", "?"], ["?", "?", "?"]]
22 self.rows = 3
23 self.columns = 3
24
25 self.x = 1
26 self.y = 1
27 self.previous_x = 1
28 self.previous_y = 1
29 self.heading = "right"
30 self.previous_heading = "right"
31
32 # key - current direction, value - new direction after turning right/left
33 self.turn_right_headings = {"left": "up", "up": "right", "right": "down", "down": "left"}
34 self.turn_left_headings = {"left": "down", "down": "right", "right": "up", "up": "left"}
35
36 # key - cell (x, y), value - number of neighbouring unexplored cells
37 self.cells = {(0, 0): 4}
38
39 def set_robot(self, robot: PiBot.PiBot()) -> None:
40 """Set the API reference."""
41 self.robot = robot
42
43 def update_pose(self):
44 """Update the robot pose using the sensor values."""
45 if self.l_enc > self.last_l_enc and self.r_enc > self.last_r_enc:
46 self.drive_forwards_or_backwards(1) # forwards
47 elif self.l_enc < self.last_l_enc and self.r_enc < self.last_r_enc:
48 self.drive_forwards_or_backwards(-1) # backwards
49 elif self.l_enc > self.last_l_enc and self.r_enc < self.last_r_enc:
50 self.heading = self.turn_right_headings.get(self.heading) # turn right - change heading
51 elif self.l_enc < self.last_l_enc and self.r_enc > self.last_r_enc:
52 self.heading = self.turn_left_headings.get(self.heading) # turn left - change heading
53
54 def drive_forwards_or_backwards(self, coefficient: int):
55 """Drive forwards (coefficient == 1) or backwards (coefficient == -1) and change coordinates accordingly."""
56 if self.heading == "right":
57 self.x += 2 * coefficient
58 elif self.heading == "up":
59 self.y += 2 * coefficient
60 elif self.heading == "left":
61 self.x -= 2 * coefficient
62 elif self.heading == "down":
63 self.y -= 2 * coefficient
64
65 def update_map(self) -> None:
66 """Update the internal map using the sensor values."""
67 updated_rows_columns = self.update_rows_and_columns() # find new (if any) rows and columns
68 self.add_rows_and_columns(updated_rows_columns[0], updated_rows_columns[1]) # add these new rows/columns to map
69 self.rows += updated_rows_columns[0] # update the rows variable
70 self.columns += updated_rows_columns[1] # update the columns variable
71 self.add_empty_spaces() # add ' ' places to map
72 self.add_a_wall() # add X places to map
73 self.add_cells() # add cells to dictionary
74 self.update_cells() # update the cells
75
76 def new_rows_and_columns(self):
77 """Get the number of new rows and columns."""
78 new_rows = 0
79 new_columns = 0
80 if self.x > self.previous_x and self.y == self.previous_y:
81 new_columns += 2
82 elif self.y > self.previous_y and self.x == self.previous_x:
83 new_rows += 2
84
85 if self.heading == "right":
86 new_columns += 2 * self.front_middle_laser
87 elif self.heading == "up":
88 new_rows += 2 * self.front_middle_laser
89 return new_rows, new_columns
90
91 def update_rows_and_columns(self):
92 """Update rows and columns."""
93 rows = 0
94 columns = 0
95 if self.x > len(self.map[0]) - 1 or self.y > len(self.map) - 1:
96 new_rows_and_columns = self.new_rows_and_columns()
97 rows = new_rows_and_columns[0]
98 columns = new_rows_and_columns[1]
99 return rows, columns
100
101 def add_rows_and_columns(self, rows, columns):
102 """Add new rows and columns."""
103 while rows > 0:
104 self.map.append(["?"] * self.columns)
105 rows -= 1
106 for row in self.map:
107 counter = columns
108 while counter > 0:
109 row.append("?")
110 counter -= 1
111
112 def add_empty_spaces(self):
113 """Add an empty space to current position and then according to laser info."""
114 self.map[self.y][self.x] = " " # add a space to the current position
115 laser = self.front_middle_laser
116 while laser > 0 and self.heading == "left": # add spaces based on laser
117 self.map[self.y][self.x - laser] = " "
118 laser -= 1
119 while laser > 0 and self.heading == "right":
120 if self.x + laser > len(self.map[0]) - 1:
121 self.add_rows_and_columns(0, laser)
122 self.map[self.y][self.x + laser] = " "
123 laser -= 1
124 while laser > 0 and self.heading == "up":
125 if self.y + laser > len(self.map) - 1:
126 self.add_rows_and_columns(laser, 0)
127 self.map[self.y + laser][self.x] = " "
128 laser -= 1
129 while laser > 0 and self.heading == "down":
130 self.map[self.y - laser][self.x] = " "
131 laser -= 1
132
133 def add_space_next_to_a_wall(self, x: int, y: int):
134 """Add an empty space next to a wall."""
135 if x < self.x:
136 self.map[y][x + 1] = " "
137 elif x > self.x:
138 self.map[y][x - 1] = " "
139 elif y < self.y:
140 self.map[y + 1][x] = " "
141 elif y > self.y:
142 self.map[y - 1][x] = " "
143
144 def add_a_wall(self):
145 """Get wall coordinates."""
146 if self.front_middle_laser < 2: # start calculating wall coordinates
147 if self.heading == "left":
148 x = self.x - 1 - 2 * self.front_middle_laser
149 y = self.y
150 elif self.heading == "right":
151 x = self.x + 1 + 2 * self.front_middle_laser
152 y = self.y
153 elif self.heading == "up":
154 x = self.x
155 y = self.y + 1 + 2 * self.front_middle_laser
156 elif self.heading == "down":
157 x = self.x
158 y = self.y - 1 - 2 * self.front_middle_laser
159
160 if x > len(self.map[0]) - 1: # if the map is too small for the wall coordinates, then it expands the map
161 self.add_rows_and_columns(0, x - (len(self.map[0]) - 1))
162 if y > len(self.map):
163 self.add_rows_and_columns(y - (len(self.map) - 1), 0)
164 self.map[y][x] = "X" # add a wall
165 self.add_space_next_to_a_wall(x, y) # add space next to it
166
167 def add_cells(self):
168 """Add new cells."""
169 self.rows = len(self.map)
170 self.columns = len(self.map[0])
171 current_cell = self.calculate_map_coordinate_to_cell(self.x), self.calculate_map_coordinate_to_cell(self.y)
172 if current_cell not in self.cells: # add the current cell
173 self.cells[current_cell] = self.calculate_unexplored_neighbour_cells(current_cell)
174
175 for y in range(0, self.rows - 1): # add all cells that have not been added yet
176 for x in range(0, self.columns - 1):
177 x_cell = self.calculate_map_coordinate_to_cell(x)
178 y_cell = self.calculate_map_coordinate_to_cell(y)
179 cell = (x_cell, y_cell)
180 if cell not in self.cells:
181 self.cells[cell] = self.calculate_unexplored_neighbour_cells(cell)
182
183 def update_cells(self):
184 """Update the cells in the dictionary."""
185 for cell, unknown_neighbours in self.cells.items():
186 self.cells[cell] = self.calculate_unexplored_neighbour_cells(cell)
187
188 @staticmethod
189 def distance(first: tuple, second: tuple) -> float:
190 """Calculate the Euclidean distance between two points."""
191 return math.sqrt((first[0] - second[0]) ** 2 + (first[1] - second[1]) ** 2)
192
193 @staticmethod
194 def calculate_map_coordinate_to_cell(coordinate: int) -> int:
195 """Calculate a map coordinate (OT14) to a cell coordinate (OT15)."""
196 if coordinate % 2 == 0:
197 return coordinate // 2
198 return (coordinate - 1) // 2
199
200 @staticmethod
201 def calculate_cell_to_map_coordinate(coordinate: int) -> int:
202 """Calculate a cell coordinate (OT15) to a map coordinate (OT14)."""
203 return coordinate * 2 + 1
204
205 def calculate_unexplored_neighbour_cells(self, cell: tuple) -> int:
206 """Calculate the number of neighbouring unexplored cells.."""
207 x = self.calculate_cell_to_map_coordinate(cell[0])
208 y = self.calculate_cell_to_map_coordinate(cell[1])
209 unexplored_neighbours = 0
210 if self.map[y][x - 1] == "?":
211 unexplored_neighbours += 1
212 if self.map[y][x + 1] == "?":
213 unexplored_neighbours += 1
214 if self.map[y - 1][x] == "?":
215 unexplored_neighbours += 1
216 if self.map[y + 1][x] == "?":
217 unexplored_neighbours += 1
218 return unexplored_neighbours
219
220 def find_closest_frontier(self) -> tuple:
221 """
222 Find the closest frontier.
223
224 Returns:
225 The cell coordinates as a tuple (x, y) with the best
226 exploration value.
227 The exploration value is defined as the Euclidean distance
228 from the current robot position to the frontier (closer is better).
229 A cell is considered to be a frontier if it has neighboring cells
230 which have not been explored.
231 In case of tiebreak, the frontier with more neighboring unknown
232 cells must be preferred.
233 If this does not break the tie, the cell which is farther
234 from (0, 0) must be preferred.
235
236 If there are no frontiers, return None.
237 """
238 if self.sensed:
239 closest_frontier = None
240 max_unknown_neighbours = 0
241 min_distance = 1000
242 distance_from_zero = 0
243 current_cell = self.calculate_map_coordinate_to_cell(self.x), self.calculate_map_coordinate_to_cell(self.y)
244 for cell, unknown_neighbours in self.cells.items():
245 x_map = self.calculate_cell_to_map_coordinate(cell[0])
246 y_map = self.calculate_cell_to_map_coordinate(cell[1])
247 distance = self.distance(current_cell, cell)
248 if self.map[y_map][x_map] != "?" and unknown_neighbours > 0:
249 if distance < min_distance:
250 closest_frontier = cell
251 min_distance = distance
252 max_unknown_neighbours = unknown_neighbours
253 distance_from_zero = self.distance((0, 0), cell)
254 elif distance == min_distance:
255 if unknown_neighbours > max_unknown_neighbours:
256 closest_frontier = cell
257 min_distance = distance
258 max_unknown_neighbours = unknown_neighbours
259 distance_from_zero = self.distance((0, 0), cell)
260 elif unknown_neighbours == max_unknown_neighbours:
261 if self.distance((0, 0), cell) > distance_from_zero:
262 closest_frontier = cell
263 min_distance = distance
264 max_unknown_neighbours = unknown_neighbours
265 distance_from_zero = self.distance((0, 0), cell)
266 return closest_frontier
267 return None
268
269 def sense(self):
270 """SPA architecture sense method."""
271 self.last_l_enc = self.l_enc
272 self.last_r_enc = self.r_enc
273
274 self.l_enc = self.robot.get_left_wheel_encoder()
275 self.r_enc = self.robot.get_right_wheel_encoder()
276 self.front_middle_laser = self.robot.get_front_middle_laser()
277
278 self.rows = len(self.map)
279 self.columns = len(self.map[0])
280
281 self.previous_x = self.x
282 self.previous_y = self.y
283 self.sensed = True
284
285
286def main():
287 """The main entry point."""
288 robot = Robot()
289 for i in range(10):
290 robot.sense()
291 robot.update_pose()
292 robot.update_map()
293 print(robot.find_closest_frontier())
294 robot.robot.sleep(1)
295
296
297def test():
298 """Test."""
299 robot = Robot()
300 import spinzag # or any other data file
301 data = spinzag.get_data()
302 robot.robot.load_data_profile(data)
303 for i in range(len(data)):
304 robot.sense()
305 robot.update_pose()
306 robot.update_map()
307 print(robot.find_closest_frontier())
308 robot.robot.sleep(0.05)
309
310
311if __name__ == "__main__":
312 test()