-
Notifications
You must be signed in to change notification settings - Fork 17
/
planning_utils_from_seed_project.py
144 lines (114 loc) · 4.26 KB
/
planning_utils_from_seed_project.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
from enum import Enum
from queue import PriorityQueue
import numpy as np
def create_grid(data, drone_altitude, safety_distance):
"""
Returns a grid representation of a 2D configuration space
based on given obstacle data, drone altitude and safety distance
arguments.
"""
# minimum and maximum north coordinates
north_min = np.floor(np.min(data[:, 0] - data[:, 3]))
north_max = np.ceil(np.max(data[:, 0] + data[:, 3]))
# minimum and maximum east coordinates
east_min = np.floor(np.min(data[:, 1] - data[:, 4]))
east_max = np.ceil(np.max(data[:, 1] + data[:, 4]))
# given the minimum and maximum coordinates we can
# calculate the size of the grid.
north_size = int(np.ceil((north_max - north_min + 1)))
east_size = int(np.ceil((east_max - east_min + 1)))
# Initialize an empty grid
grid = np.zeros((north_size, east_size))
# Populate the grid with obstacles
for i in range(data.shape[0]):
north, east, alt, d_north, d_east, d_alt = data[i, :]
if alt + d_alt + safety_distance > drone_altitude:
obstacle = [
int(np.clip(north - d_north - safety_distance - north_min, 0, north_size-1)),
int(np.clip(north + d_north + safety_distance - north_min, 0, north_size-1)),
int(np.clip(east - d_east - safety_distance - east_min, 0, east_size-1)),
int(np.clip(east + d_east + safety_distance - east_min, 0, east_size-1)),
]
grid[obstacle[0]:obstacle[1]+1, obstacle[2]:obstacle[3]+1] = 1
return grid, int(north_min), int(east_min)
# Assume all actions cost the same.
class Action(Enum):
"""
An action is represented by a 3 element tuple.
The first 2 values are the delta of the action relative
to the current grid position. The third and final value
is the cost of performing the action.
"""
WEST = (0, -1, 1)
EAST = (0, 1, 1)
NORTH = (-1, 0, 1)
SOUTH = (1, 0, 1)
@property
def cost(self):
return self.value[2]
@property
def delta(self):
return (self.value[0], self.value[1])
def valid_actions(grid, current_node):
"""
Returns a list of valid actions given a grid and current node.
"""
valid_actions = list(Action)
n, m = grid.shape[0] - 1, grid.shape[1] - 1
x, y = current_node
# check if the node is off the grid or
# it's an obstacle
if x - 1 < 0 or grid[x - 1, y] == 1:
valid_actions.remove(Action.NORTH)
if x + 1 > n or grid[x + 1, y] == 1:
valid_actions.remove(Action.SOUTH)
if y - 1 < 0 or grid[x, y - 1] == 1:
valid_actions.remove(Action.WEST)
if y + 1 > m or grid[x, y + 1] == 1:
valid_actions.remove(Action.EAST)
return valid_actions
def a_star(grid, h, start, goal):
"""
Given a grid and heuristic function returns
the lowest cost path from start to goal.
"""
path = []
path_cost = 0
queue = PriorityQueue()
queue.put((0, start))
visited = set(start)
branch = {}
found = False
while not queue.empty():
item = queue.get()
current_cost = item[0]
current_node = item[1]
if current_node == goal:
print('Found a path.')
found = True
break
else:
# Get the new vertexes connected to the current vertex
for a in valid_actions(grid, current_node):
next_node = (current_node[0] + a.delta[0], current_node[1] + a.delta[1])
new_cost = current_cost + a.cost + h(next_node, goal)
if next_node not in visited:
visited.add(next_node)
queue.put((new_cost, next_node))
branch[next_node] = (new_cost, current_node, a)
if found:
# retrace steps
n = goal
path_cost = branch[n][0]
path.append(goal)
while branch[n][1] != start:
path.append(branch[n][1])
n = branch[n][1]
path.append(branch[n][1])
else:
print('**********************')
print('Failed to find a path!')
print('**********************')
return path[::-1], path_cost
def heuristic(position, goal_position):
return np.linalg.norm(np.array(position) - np.array(goal_position))