forked from cargecla/FCND-Motion-Planning
-
Notifications
You must be signed in to change notification settings - Fork 0
/
planning_utils.py
159 lines (129 loc) · 4.97 KB
/
planning_utils.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
153
154
155
156
157
158
159
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))
east_size = int(np.ceil(east_max - east_min))
# 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)
# Add diagonal motions with a cost of sqrt(2)
WEST_NORTH = (-1, -1, np.sqrt(2))
WEST_SOUTH = (1, -1, np.sqrt(2))
EAST_NORTH = (-1, 1, np.sqrt(2))
EAST_SOUTH = (1, 1, np.sqrt(2))
@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)
# Add diagonal motions as valid actions
if x - 1 < 0 or y - 1 < 0 or grid[x - 1, y - 1] == 1:
valid_actions.remove(Action.WEST_NORTH)
if x + 1 > n or y - 1 < 0 or grid[x + 1, y - 1] == 1:
valid_actions.remove(Action.WEST_SOUTH)
if x - 1 < 0 or y + 1 > m or grid[x - 1, y + 1] == 1:
valid_actions.remove(Action.EAST_NORTH)
if x + 1 > n or y +1 > m or grid[x + 1, y + 1] == 1:
valid_actions.remove(Action.EAST_SOUTH)
return valid_actions
def a_star(grid, h, start, 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_node = item[1]
if current_node == start:
current_cost = 0.0
else:
current_cost = branch[current_node][0]
if current_node == goal:
print('Found a path.')
found = True
break
else:
for action in valid_actions(grid, current_node):
# get the tuple representation
da = action.delta
next_node = (current_node[0] + da[0], current_node[1] + da[1])
branch_cost = current_cost + action.cost
queue_cost = branch_cost + h(next_node, goal)
if next_node not in visited:
visited.add(next_node)
branch[next_node] = (branch_cost, current_node, action)
queue.put((queue_cost, next_node))
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))