def multi_query(self, n, verbose=False):
if verbose:
print('Start multi_query')
t0 = time.time()
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, self.world.start)
goal_prm = self.find_nearest_node(self.V, self.world.goal)
# Initialize Dijkstra module
djk1 = Dijkstra(self.V, self.E)
djk2 = Dijkstra(self.V, self.E)
# Build a distance map
djk1.build(start_prm)
djk2.build(goal_prm)
if verbose:
t1 = time.time()
print('Build a distance map: {}'.format(t1 - t0))
# Generate multiple paths
self.path_list = []
while len(self.path_list) < n:
mid_point = np.random.randint(len(self.V))
djk1.query(mid_point)
djk2.query(mid_point)
if djk1.path is not None and djk2.path is not None:
djk1.path = np.vstack((self.world.start, djk1.path))
djk2.path = np.vstack((djk2.path[-2::-1], self.world.goal))
'''
smoothed_path1 = self.smoothing(djk1.path)
smoothed_path2 = self.smoothing(djk2.path)
self.smoothed_path = np.vstack((
smoothed_path1,
smoothed_path2))
'''
self.path = np.vstack((djk1.path, djk2.path))
self.smoothed_path = self.smoothing(self.path)
# '''
self.path_list.append(self.smoothed_path)
if verbose:
t2 = time.time()
print('Generate multiple paths: {}\n'.format(t2 - t1))
def query(self, start, goal):
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, start)
goal_prm = self.find_nearest_node(self.V, goal)
# If nearest node cannot be found, self.path = None
if start_prm is None or goal_prm is None:
self.path = None
# else, find shortest path (Dijkstra's algorithm)
else:
djk = Dijkstra(self.V, self.E)
djk.build(start_prm)
djk.query(goal_prm)
if djk.path is not None:
self.path = np.vstack(
(self.world.start, djk.path, self.world.goal))
else:
self.path = None
def create_pop_cd(n_ind):
m = 6
n = 6
w = 1.0
h = 1.0
x0 = -3.0
y0 = -3.0
# Nodes
V = [[x0 + i * w, y0 + j * h] for j in range(n) for i in range(m)]
# Edges
E = []
for i in range(m * n - 1):
if i % m != (m - 1):
E.append([(i, i + 1), 1.0])
if int(i / m) != (n - 1):
E.append([(i, i + n), 1.0])
obj_cel = [6, 7, 8, 9, 26, 27, 28, 29]
E = [e for e in E if (e[0][0] not in obj_cel) and (e[0][1] not in obj_cel)]
# Find shortest path (Dijkstra)
djk = Dijkstra(V, E)
djk.build(0)
djk.query(35)
# Create Population
pop = []
for i in range(n_ind):
path = [world.start]
for j in range(1, len(djk.path) - 1):
x = np.random.rand() * w + djk.path[j][0]
y = np.random.rand() * h + djk.path[j][1]
path.append([x, y])
path.append(world.goal)
shortened_path = post_process.shortcut(path, world)
pop.append(Individual(shortened_path))
return pop
y0 = -3.0
V = [[x0 + i * w, y0 + j * h] for j in range(n) for i in range(m)]
E = []
for i in range(m * n - 1):
if i % m != (m - 1):
E.append([(i, i + 1), 1.0])
if int(i / m) != (n - 1):
E.append([(i, i + n), 1.0])
obj_cel = [6, 7, 8, 9, 26, 27, 28, 29]
E = [e for e in E if (e[0][0] not in obj_cel) and (e[0][1] not in obj_cel)]
# In[]:
djk = Dijkstra(V, E)
djk.build(0)
djk.query(35)
# In[]:
path = []
for cell in djk.path:
x = np.random.rand() * w + cell[0]
y = np.random.rand() * h + cell[1]
path.append([x, y])
pts = np.array(path)
pts = np.vstack((world.start, pts, world.goal))
# In[]:
pts_smooth = post_process.shortcut(pts, world)
# In[]:
