|
| 1 | +""" |
| 2 | +flowfield pathfinding |
| 3 | +author: Sarim Mehdi (muhammadsarim.mehdi@studio.unibo.it) |
| 4 | +Source: https://leifnode.com/2013/12/flow-field-pathfinding/ |
| 5 | +""" |
| 6 | + |
| 7 | +import numpy as np |
| 8 | +import matplotlib.pyplot as plt |
| 9 | + |
| 10 | +show_animation = True |
| 11 | + |
| 12 | + |
| 13 | +def draw_horizontal_line(start_x, start_y, length, o_x, o_y, o_dict, path): |
| 14 | + for i in range(start_x, start_x + length): |
| 15 | + for j in range(start_y, start_y + 2): |
| 16 | + o_x.append(i) |
| 17 | + o_y.append(j) |
| 18 | + o_dict[(i, j)] = path |
| 19 | + |
| 20 | + |
| 21 | +def draw_vertical_line(start_x, start_y, length, o_x, o_y, o_dict, path): |
| 22 | + for i in range(start_x, start_x + 2): |
| 23 | + for j in range(start_y, start_y + length): |
| 24 | + o_x.append(i) |
| 25 | + o_y.append(j) |
| 26 | + o_dict[(i, j)] = path |
| 27 | + |
| 28 | + |
| 29 | +class FlowField: |
| 30 | + def __init__(self, obs_grid, goal_x, goal_y, start_x, start_y, |
| 31 | + limit_x, limit_y): |
| 32 | + self.start_pt = [start_x, start_y] |
| 33 | + self.goal_pt = [goal_x, goal_y] |
| 34 | + self.obs_grid = obs_grid |
| 35 | + self.limit_x, self.limit_y = limit_x, limit_y |
| 36 | + self.cost_field = {} |
| 37 | + self.integration_field = {} |
| 38 | + self.vector_field = {} |
| 39 | + |
| 40 | + def find_path(self): |
| 41 | + self.create_cost_field() |
| 42 | + self.create_integration_field() |
| 43 | + self.assign_vectors() |
| 44 | + self.follow_vectors() |
| 45 | + |
| 46 | + def create_cost_field(self): |
| 47 | + """Assign cost to each grid which defines the energy |
| 48 | + it would take to get there.""" |
| 49 | + for i in range(self.limit_x): |
| 50 | + for j in range(self.limit_y): |
| 51 | + if self.obs_grid[(i, j)] == 'free': |
| 52 | + self.cost_field[(i, j)] = 1 |
| 53 | + elif self.obs_grid[(i, j)] == 'medium': |
| 54 | + self.cost_field[(i, j)] = 7 |
| 55 | + elif self.obs_grid[(i, j)] == 'hard': |
| 56 | + self.cost_field[(i, j)] = 20 |
| 57 | + elif self.obs_grid[(i, j)] == 'obs': |
| 58 | + continue |
| 59 | + |
| 60 | + if [i, j] == self.goal_pt: |
| 61 | + self.cost_field[(i, j)] = 0 |
| 62 | + |
| 63 | + def create_integration_field(self): |
| 64 | + """Start from the goal node and calculate the value |
| 65 | + of the integration field at each node. Start by |
| 66 | + assigning a value of infinity to every node except |
| 67 | + the goal node which is assigned a value of 0. Put the |
| 68 | + goal node in the open list and then get its neighbors |
| 69 | + (must not be obstacles). For each neighbor, the new |
| 70 | + cost is equal to the cost of the current node in the |
| 71 | + integration field (in the beginning, this will simply |
| 72 | + be the goal node) + the cost of the neighbor in the |
| 73 | + cost field + the extra cost (optional). The new cost |
| 74 | + is only assigned if it is less than the previously |
| 75 | + assigned cost of the node in the integration field and, |
| 76 | + when that happens, the neighbor is put on the open list. |
| 77 | + This process continues until the open list is empty.""" |
| 78 | + for i in range(self.limit_x): |
| 79 | + for j in range(self.limit_y): |
| 80 | + if self.obs_grid[(i, j)] == 'obs': |
| 81 | + continue |
| 82 | + self.integration_field[(i, j)] = np.inf |
| 83 | + if [i, j] == self.goal_pt: |
| 84 | + self.integration_field[(i, j)] = 0 |
| 85 | + |
| 86 | + open_list = [(self.goal_pt, 0)] |
| 87 | + while open_list: |
| 88 | + curr_pos, curr_cost = open_list[0] |
| 89 | + curr_x, curr_y = curr_pos |
| 90 | + for i in range(-1, 2): |
| 91 | + for j in range(-1, 2): |
| 92 | + x, y = curr_x + i, curr_y + j |
| 93 | + if self.obs_grid[(x, y)] == 'obs': |
| 94 | + continue |
| 95 | + if (i, j) in [(1, 0), (0, 1), (-1, 0), (0, -1)]: |
| 96 | + e_cost = 10 |
| 97 | + else: |
| 98 | + e_cost = 14 |
| 99 | + neighbor_energy = self.cost_field[(x, y)] |
| 100 | + neighbor_old_cost = self.integration_field[(x, y)] |
| 101 | + neighbor_new_cost = curr_cost + neighbor_energy + e_cost |
| 102 | + if neighbor_new_cost < neighbor_old_cost: |
| 103 | + self.integration_field[(x, y)] = neighbor_new_cost |
| 104 | + open_list.append(([x, y], neighbor_new_cost)) |
| 105 | + del open_list[0] |
| 106 | + |
| 107 | + def assign_vectors(self): |
| 108 | + """For each node, assign a vector from itself to the node with |
| 109 | + the lowest cost in the integration field. An agent will simply |
| 110 | + follow this vector field to the goal""" |
| 111 | + for i in range(self.limit_x): |
| 112 | + for j in range(self.limit_y): |
| 113 | + if self.obs_grid[(i, j)] == 'obs': |
| 114 | + continue |
| 115 | + if [i, j] == self.goal_pt: |
| 116 | + self.vector_field[(i, j)] = (None, None) |
| 117 | + continue |
| 118 | + offset_list = [(i + a, j + b) |
| 119 | + for a in range(-1, 2) |
| 120 | + for b in range(-1, 2)] |
| 121 | + neighbor_list = [{'loc': pt, |
| 122 | + 'cost': self.integration_field[pt]} |
| 123 | + for pt in offset_list |
| 124 | + if self.obs_grid[pt] != 'obs'] |
| 125 | + neighbor_list = sorted(neighbor_list, key=lambda x: x['cost']) |
| 126 | + best_neighbor = neighbor_list[0]['loc'] |
| 127 | + self.vector_field[(i, j)] = best_neighbor |
| 128 | + |
| 129 | + def follow_vectors(self): |
| 130 | + curr_x, curr_y = self.start_pt |
| 131 | + while curr_x is not None and curr_y is not None: |
| 132 | + plt.plot(curr_x, curr_y, "b*") |
| 133 | + curr_x, curr_y = self.vector_field[(curr_x, curr_y)] |
| 134 | + plt.pause(0.001) |
| 135 | + if show_animation: |
| 136 | + plt.show() |
| 137 | + |
| 138 | + |
| 139 | +def main(): |
| 140 | + # set obstacle positions |
| 141 | + obs_dict = {} |
| 142 | + for i in range(51): |
| 143 | + for j in range(51): |
| 144 | + obs_dict[(i, j)] = 'free' |
| 145 | + o_x, o_y, m_x, m_y, h_x, h_y = [], [], [], [], [], [] |
| 146 | + |
| 147 | + s_x = 5.0 |
| 148 | + s_y = 5.0 |
| 149 | + g_x = 35.0 |
| 150 | + g_y = 45.0 |
| 151 | + |
| 152 | + # draw outer border of maze |
| 153 | + draw_vertical_line(0, 0, 50, o_x, o_y, obs_dict, 'obs') |
| 154 | + draw_vertical_line(48, 0, 50, o_x, o_y, obs_dict, 'obs') |
| 155 | + draw_horizontal_line(0, 0, 50, o_x, o_y, obs_dict, 'obs') |
| 156 | + draw_horizontal_line(0, 48, 50, o_x, o_y, obs_dict, 'obs') |
| 157 | + |
| 158 | + # draw inner walls |
| 159 | + all_x = [10, 10, 10, 15, 20, 20, 30, 30, 35, 30, 40, 45] |
| 160 | + all_y = [10, 30, 45, 20, 5, 40, 10, 40, 5, 40, 10, 25] |
| 161 | + all_len = [10, 10, 5, 10, 10, 5, 20, 10, 25, 10, 35, 15] |
| 162 | + for x, y, l in zip(all_x, all_y, all_len): |
| 163 | + draw_vertical_line(x, y, l, o_x, o_y, obs_dict, 'obs') |
| 164 | + |
| 165 | + all_x[:], all_y[:], all_len[:] = [], [], [] |
| 166 | + all_x = [35, 40, 15, 10, 45, 20, 10, 15, 25, 45, 10, 30, 10, 40] |
| 167 | + all_y = [5, 10, 15, 20, 20, 25, 30, 35, 35, 35, 40, 40, 45, 45] |
| 168 | + all_len = [10, 5, 10, 10, 5, 5, 10, 5, 10, 5, 10, 5, 5, 5] |
| 169 | + for x, y, l in zip(all_x, all_y, all_len): |
| 170 | + draw_horizontal_line(x, y, l, o_x, o_y, obs_dict, 'obs') |
| 171 | + |
| 172 | + # Some points are assigned a slightly higher energy value in the cost |
| 173 | + # field. For example, if an agent wishes to go to a point, it might |
| 174 | + # encounter different kind of terrain like grass and dirt. Grass is |
| 175 | + # assigned medium difficulty of passage (color coded as green on the |
| 176 | + # map here). Dirt is assigned hard difficulty of passage (color coded |
| 177 | + # as brown here). Hence, this algorithm will take into account how |
| 178 | + # difficult it is to go through certain areas of a map when deciding |
| 179 | + # the shortest path. |
| 180 | + |
| 181 | + # draw paths that have medium difficulty (in terms of going through them) |
| 182 | + all_x[:], all_y[:], all_len[:] = [], [], [] |
| 183 | + all_x = [10, 45] |
| 184 | + all_y = [22, 20] |
| 185 | + all_len = [8, 5] |
| 186 | + for x, y, l in zip(all_x, all_y, all_len): |
| 187 | + draw_vertical_line(x, y, l, m_x, m_y, obs_dict, 'medium') |
| 188 | + |
| 189 | + all_x[:], all_y[:], all_len[:] = [], [], [] |
| 190 | + all_x = [20, 30, 42] + [47] * 5 |
| 191 | + all_y = [35, 30, 38] + [37 + i for i in range(2)] |
| 192 | + all_len = [5, 7, 3] + [1] * 3 |
| 193 | + for x, y, l in zip(all_x, all_y, all_len): |
| 194 | + draw_horizontal_line(x, y, l, m_x, m_y, obs_dict, 'medium') |
| 195 | + |
| 196 | + # draw paths that have hard difficulty (in terms of going through them) |
| 197 | + all_x[:], all_y[:], all_len[:] = [], [], [] |
| 198 | + all_x = [15, 20, 35] |
| 199 | + all_y = [45, 20, 35] |
| 200 | + all_len = [3, 5, 7] |
| 201 | + for x, y, l in zip(all_x, all_y, all_len): |
| 202 | + draw_vertical_line(x, y, l, h_x, h_y, obs_dict, 'hard') |
| 203 | + |
| 204 | + all_x[:], all_y[:], all_len[:] = [], [], [] |
| 205 | + all_x = [30] + [47] * 5 |
| 206 | + all_y = [10] + [37 + i for i in range(2)] |
| 207 | + all_len = [5] + [1] * 3 |
| 208 | + for x, y, l in zip(all_x, all_y, all_len): |
| 209 | + draw_horizontal_line(x, y, l, h_x, h_y, obs_dict, 'hard') |
| 210 | + |
| 211 | + plt.plot(o_x, o_y, "sr") |
| 212 | + plt.plot(m_x, m_y, "sg") |
| 213 | + plt.plot(h_x, h_y, "sy") |
| 214 | + plt.plot(s_x, s_y, "og") |
| 215 | + plt.plot(g_x, g_y, "o") |
| 216 | + plt.grid(True) |
| 217 | + |
| 218 | + flow_obj = FlowField(obs_dict, g_x, g_y, s_x, s_y, 50, 50) |
| 219 | + flow_obj.find_path() |
| 220 | + |
| 221 | + |
| 222 | +if __name__ == '__main__': |
| 223 | + main() |
0 commit comments