mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 01:28:23 -05:00
fix scanning error (#339)
This commit is contained in:
Atsushi Sakai
@@ -27,7 +27,8 @@ class RTree(object):
|
||||
# Class to represent the explicit tree created
|
||||
# while sampling through the state space
|
||||
|
||||
def __init__(self, start=None, lowerLimit=None, upperLimit=None, resolution=1.0):
|
||||
def __init__(self, start=None, lowerLimit=None, upperLimit=None,
|
||||
resolution=1.0):
|
||||
|
||||
if upperLimit is None:
|
||||
upperLimit = [10, 10]
|
||||
@@ -96,10 +97,11 @@ class RTree(object):
|
||||
def real_world_to_node_id(self, real_coord):
|
||||
# first convert the given coordinates to grid space and then
|
||||
# convert the grid space coordinates to a unique node id
|
||||
return self.grid_coordinate_to_node_id(self.real_coords_to_grid_coord(real_coord))
|
||||
return self.grid_coordinate_to_node_id(
|
||||
self.real_coords_to_grid_coord(real_coord))
|
||||
|
||||
def grid_coord_to_real_world_coord(self, coord):
|
||||
# This function smaps a grid coordinate in discrete space
|
||||
# This function maps a grid coordinate in discrete space
|
||||
# to a configuration in the full configuration space
|
||||
config = [0] * self.dimension
|
||||
for i in range(0, len(coord)):
|
||||
@@ -126,7 +128,8 @@ class RTree(object):
|
||||
def node_id_to_real_world_coord(self, nid):
|
||||
# This function maps a node in discrete space to a configuration
|
||||
# in the full configuration space
|
||||
return self.grid_coord_to_real_world_coord(self.node_id_to_grid_coord(nid))
|
||||
return self.grid_coord_to_real_world_coord(
|
||||
self.node_id_to_grid_coord(nid))
|
||||
|
||||
|
||||
class BITStar(object):
|
||||
@@ -137,9 +140,9 @@ class BITStar(object):
|
||||
self.start = start
|
||||
self.goal = goal
|
||||
|
||||
self.minrand = randArea[0]
|
||||
self.maxrand = randArea[1]
|
||||
self.maxIter = maxIter
|
||||
self.min_rand = randArea[0]
|
||||
self.max_rand = randArea[1]
|
||||
self.max_iIter = maxIter
|
||||
self.obstacleList = obstacleList
|
||||
self.startId = None
|
||||
self.goalId = None
|
||||
@@ -186,18 +189,19 @@ class BITStar(object):
|
||||
[(self.start[1] + self.goal[1]) / 2.0], [0]])
|
||||
a1 = np.array([[(self.goal[0] - self.start[0]) / cMin],
|
||||
[(self.goal[1] - self.start[1]) / cMin], [0]])
|
||||
etheta = math.atan2(a1[1], a1[0])
|
||||
# first column of idenity matrix transposed
|
||||
eTheta = math.atan2(a1[1], a1[0])
|
||||
# first column of identity matrix transposed
|
||||
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
|
||||
M = np.dot(a1, id1_t)
|
||||
U, S, Vh = np.linalg.svd(M, True, True)
|
||||
C = np.dot(np.dot(U, np.diag(
|
||||
[1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh)
|
||||
[1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
|
||||
Vh)
|
||||
|
||||
self.samples.update(self.informed_sample(
|
||||
200, cBest, cMin, xCenter, C))
|
||||
|
||||
return etheta, cMin, xCenter, C, cBest
|
||||
return eTheta, cMin, xCenter, C, cBest
|
||||
|
||||
def setup_sample(self, iterations, foundGoal, cMin, xCenter, C, cBest):
|
||||
|
||||
@@ -228,18 +232,19 @@ class BITStar(object):
|
||||
|
||||
def plan(self, animation=True):
|
||||
|
||||
etheta, cMin, xCenter, C, cBest = self.setup_planning()
|
||||
eTheta, cMin, xCenter, C, cBest = self.setup_planning()
|
||||
iterations = 0
|
||||
|
||||
foundGoal = False
|
||||
# run until done
|
||||
while iterations < self.maxIter:
|
||||
while iterations < self.max_iIter:
|
||||
cBest = self.setup_sample(iterations,
|
||||
foundGoal, cMin, xCenter, C, cBest)
|
||||
# expand the best vertices until an edge is better than the vertex
|
||||
# this is done because the vertex cost represents the lower bound
|
||||
# on the edge cost
|
||||
while self.best_vertex_queue_value() <= self.best_edge_queue_value():
|
||||
while self.best_vertex_queue_value() <= \
|
||||
self.best_edge_queue_value():
|
||||
self.expand_vertex(self.best_in_vertex_queue())
|
||||
|
||||
# add the best edge to the tree
|
||||
@@ -247,11 +252,17 @@ class BITStar(object):
|
||||
self.edge_queue.remove(bestEdge)
|
||||
|
||||
# Check if this can improve the current solution
|
||||
estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.compute_distance_cost(
|
||||
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId)
|
||||
estimatedCostOfEdge = self.compute_distance_cost(self.startId, bestEdge[0]) + self.compute_heuristic_cost(
|
||||
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId)
|
||||
actualCostOfEdge = self.g_scores[bestEdge[0]] + self.compute_distance_cost(bestEdge[0], bestEdge[1])
|
||||
estimatedCostOfVertex = self.g_scores[bestEdge[
|
||||
0]] + self.compute_distance_cost(
|
||||
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(
|
||||
bestEdge[1], self.goalId)
|
||||
estimatedCostOfEdge = self.compute_distance_cost(
|
||||
self.startId, bestEdge[0]) + self.compute_heuristic_cost(
|
||||
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(
|
||||
bestEdge[1], self.goalId)
|
||||
actualCostOfEdge = self.g_scores[
|
||||
bestEdge[0]] + self.compute_distance_cost(
|
||||
bestEdge[0], bestEdge[1])
|
||||
|
||||
f1 = estimatedCostOfVertex < self.g_scores[self.goalId]
|
||||
f2 = estimatedCostOfEdge < self.g_scores[self.goalId]
|
||||
@@ -277,10 +288,12 @@ class BITStar(object):
|
||||
try:
|
||||
del self.samples[bestEdge[1]]
|
||||
except KeyError:
|
||||
# invalid sample key
|
||||
pass
|
||||
eid = self.tree.add_vertex(nextCoord)
|
||||
self.vertex_queue.append(eid)
|
||||
if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId:
|
||||
if eid == self.goalId or bestEdge[0] == self.goalId or \
|
||||
bestEdge[1] == self.goalId:
|
||||
print("Goal found")
|
||||
foundGoal = True
|
||||
|
||||
@@ -288,14 +301,18 @@ class BITStar(object):
|
||||
|
||||
g_score = self.compute_distance_cost(
|
||||
bestEdge[0], bestEdge[1])
|
||||
self.g_scores[bestEdge[1]] = g_score + self.g_scores[bestEdge[0]]
|
||||
self.f_scores[bestEdge[1]] = g_score + self.compute_heuristic_cost(bestEdge[1], self.goalId)
|
||||
self.g_scores[bestEdge[1]] = g_score + self.g_scores[
|
||||
bestEdge[0]]
|
||||
self.f_scores[
|
||||
bestEdge[1]] = g_score + self.compute_heuristic_cost(
|
||||
bestEdge[1], self.goalId)
|
||||
self.update_graph()
|
||||
|
||||
# visualize new edge
|
||||
if animation:
|
||||
self.draw_graph(xCenter=xCenter, cBest=cBest,
|
||||
cMin=cMin, etheta=etheta, samples=self.samples.values(),
|
||||
cMin=cMin, eTheta=eTheta,
|
||||
samples=self.samples.values(),
|
||||
start=firstCoord, end=secondCoord)
|
||||
|
||||
self.remove_queue(lastEdge, bestEdge)
|
||||
@@ -330,7 +347,8 @@ class BITStar(object):
|
||||
for edge in self.edge_queue:
|
||||
if edge[1] == bestEdge[1]:
|
||||
dist_cost = self.compute_distance_cost(edge[1], bestEdge[1])
|
||||
if self.g_scores[edge[1]] + dist_cost >= self.g_scores[self.goalId]:
|
||||
if self.g_scores[edge[1]] + dist_cost >= \
|
||||
self.g_scores[self.goalId]:
|
||||
if (lastEdge, bestEdge[1]) in self.edge_queue:
|
||||
self.edge_queue.remove(
|
||||
(lastEdge, bestEdge[1]))
|
||||
@@ -338,8 +356,9 @@ class BITStar(object):
|
||||
def connect(self, start, end):
|
||||
# A function which attempts to extend from a start coordinates
|
||||
# to goal coordinates
|
||||
steps = int(self.compute_distance_cost(self.tree.real_world_to_node_id(
|
||||
start), self.tree.real_world_to_node_id(end)) * 10)
|
||||
steps = int(self.compute_distance_cost(
|
||||
self.tree.real_world_to_node_id(start),
|
||||
self.tree.real_world_to_node_id(end)) * 10)
|
||||
x = np.linspace(start[0], end[0], num=steps)
|
||||
y = np.linspace(start[1], end[1], num=steps)
|
||||
for i in range(len(x)):
|
||||
@@ -409,8 +428,8 @@ class BITStar(object):
|
||||
return np.array([[sample[0]], [sample[1]], [0]])
|
||||
|
||||
def sample_free_space(self):
|
||||
rnd = [random.uniform(self.minrand, self.maxrand),
|
||||
random.uniform(self.minrand, self.maxrand)]
|
||||
rnd = [random.uniform(self.min_rand, self.max_rand),
|
||||
random.uniform(self.min_rand, self.max_rand)]
|
||||
|
||||
return rnd
|
||||
|
||||
@@ -418,7 +437,8 @@ class BITStar(object):
|
||||
if len(self.vertex_queue) == 0:
|
||||
return float('inf')
|
||||
values = [self.g_scores[v]
|
||||
+ self.compute_heuristic_cost(v, self.goalId) for v in self.vertex_queue]
|
||||
+ self.compute_heuristic_cost(v, self.goalId) for v in
|
||||
self.vertex_queue]
|
||||
values.sort()
|
||||
return values[0]
|
||||
|
||||
@@ -427,21 +447,25 @@ class BITStar(object):
|
||||
return float('inf')
|
||||
# return the best value in the queue by score g_tau[v] + c(v,x) + h(x)
|
||||
values = [self.g_scores[e[0]] + self.compute_distance_cost(e[0], e[1])
|
||||
+ self.compute_heuristic_cost(e[1], self.goalId) for e in self.edge_queue]
|
||||
+ self.compute_heuristic_cost(e[1], self.goalId) for e in
|
||||
self.edge_queue]
|
||||
values.sort(reverse=True)
|
||||
return values[0]
|
||||
|
||||
def best_in_vertex_queue(self):
|
||||
# return the best value in the vertex queue
|
||||
v_plus_vals = [(v, self.g_scores[v] + self.compute_heuristic_cost(v, self.goalId))
|
||||
for v in self.vertex_queue]
|
||||
v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1])
|
||||
# print(v_plus_vals)
|
||||
return v_plus_vals[0][0]
|
||||
v_plus_values = [(v, self.g_scores[v] +
|
||||
self.compute_heuristic_cost(v, self.goalId))
|
||||
for v in self.vertex_queue]
|
||||
v_plus_values = sorted(v_plus_values, key=lambda x: x[1])
|
||||
# print(v_plus_values)
|
||||
return v_plus_values[0][0]
|
||||
|
||||
def best_in_edge_queue(self):
|
||||
e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.compute_distance_cost(
|
||||
e[0], e[1]) + self.compute_heuristic_cost(e[1], self.goalId)) for e in self.edge_queue]
|
||||
e_and_values = [
|
||||
(e[0], e[1], self.g_scores[e[0]] + self.compute_distance_cost(
|
||||
e[0], e[1]) + self.compute_heuristic_cost(e[1], self.goalId))
|
||||
for e in self.edge_queue]
|
||||
e_and_values = sorted(e_and_values, key=lambda x: x[2])
|
||||
|
||||
return e_and_values[0][0], e_and_values[0][1]
|
||||
@@ -454,18 +478,19 @@ class BITStar(object):
|
||||
|
||||
# get the nearest value in vertex for every one in samples where difference is
|
||||
# less than the radius
|
||||
neigbors = []
|
||||
for sid, scoord in self.samples.items():
|
||||
scoord = np.array(scoord)
|
||||
if np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid:
|
||||
neigbors.append((sid, scoord))
|
||||
neighbors = []
|
||||
for sid, s_coord in self.samples.items():
|
||||
s_coord = np.array(s_coord)
|
||||
if np.linalg.norm(s_coord - currCoord, 2) <= self.r and sid != vid:
|
||||
neighbors.append((sid, s_coord))
|
||||
|
||||
# add an edge to the edge queue is the path might improve the solution
|
||||
for neighbor in neigbors:
|
||||
for neighbor in neighbors:
|
||||
sid = neighbor[0]
|
||||
h_cost = self.compute_heuristic_cost(sid, self.goalId)
|
||||
estimated_f_score = self.compute_distance_cost(
|
||||
self.startId, vid) + h_cost + self.compute_distance_cost(vid, sid)
|
||||
self.startId, vid) + h_cost + self.compute_distance_cost(vid,
|
||||
sid)
|
||||
if estimated_f_score < self.g_scores[self.goalId]:
|
||||
self.edge_queue.append((vid, sid))
|
||||
|
||||
@@ -476,17 +501,21 @@ class BITStar(object):
|
||||
if vid not in self.old_vertices:
|
||||
neighbors = []
|
||||
for v, edges in self.tree.vertices.items():
|
||||
if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue:
|
||||
if v != vid and (v, vid) not in self.edge_queue and \
|
||||
(vid, v) not in self.edge_queue:
|
||||
v_coord = self.tree.node_id_to_real_world_coord(v)
|
||||
if np.linalg.norm(currCoord - v_coord, 2) <= self.r:
|
||||
neighbors.append((vid, v_coord))
|
||||
|
||||
for neighbor in neighbors:
|
||||
sid = neighbor[0]
|
||||
estimated_f_score = self.compute_distance_cost(self.startId, vid) + self.compute_distance_cost(
|
||||
estimated_f_score = self.compute_distance_cost(
|
||||
self.startId, vid) + self.compute_distance_cost(
|
||||
vid, sid) + self.compute_heuristic_cost(sid, self.goalId)
|
||||
if estimated_f_score < self.g_scores[self.goalId] and (
|
||||
self.g_scores[vid] + self.compute_distance_cost(vid, sid)) < self.g_scores[sid]:
|
||||
self.g_scores[vid] +
|
||||
self.compute_distance_cost(vid, sid)) < \
|
||||
self.g_scores[sid]:
|
||||
self.edge_queue.append((vid, sid))
|
||||
|
||||
def update_graph(self):
|
||||
@@ -511,37 +540,40 @@ class BITStar(object):
|
||||
|
||||
# find a non visited successor to the current node
|
||||
successors = self.tree.vertices[currId]
|
||||
for succesor in successors:
|
||||
if succesor in closedSet:
|
||||
for successor in successors:
|
||||
if successor in closedSet:
|
||||
continue
|
||||
else:
|
||||
# claculate tentative g score
|
||||
# calculate tentative g score
|
||||
g_score = self.g_scores[currId] + \
|
||||
self.compute_distance_cost(currId, succesor)
|
||||
if succesor not in openSet:
|
||||
self.compute_distance_cost(currId, successor)
|
||||
if successor not in openSet:
|
||||
# add the successor to open set
|
||||
openSet.append(succesor)
|
||||
elif g_score >= self.g_scores[succesor]:
|
||||
openSet.append(successor)
|
||||
elif g_score >= self.g_scores[successor]:
|
||||
continue
|
||||
|
||||
# update g and f scores
|
||||
self.g_scores[succesor] = g_score
|
||||
self.f_scores[succesor] = g_score + self.compute_heuristic_cost(succesor, self.goalId)
|
||||
self.g_scores[successor] = g_score
|
||||
self.f_scores[
|
||||
successor] = g_score + self.compute_heuristic_cost(
|
||||
successor, self.goalId)
|
||||
|
||||
# store the parent and child
|
||||
self.nodes[succesor] = currId
|
||||
self.nodes[successor] = currId
|
||||
|
||||
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
|
||||
def draw_graph(self, xCenter=None, cBest=None, cMin=None, eTheta=None,
|
||||
samples=None, start=None, end=None):
|
||||
plt.clf()
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.gcf().canvas.mpl_connect(
|
||||
'key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
for rnd in samples:
|
||||
if rnd is not None:
|
||||
plt.plot(rnd[0], rnd[1], "^k")
|
||||
if cBest != float('inf'):
|
||||
self.plot_ellipse(xCenter, cBest, cMin, etheta)
|
||||
self.plot_ellipse(xCenter, cBest, cMin, eTheta)
|
||||
|
||||
if start is not None and end is not None:
|
||||
plt.plot([start[0], start[1]], [end[0], end[1]], "-g")
|
||||
@@ -556,11 +588,11 @@ class BITStar(object):
|
||||
plt.pause(0.01)
|
||||
|
||||
@staticmethod
|
||||
def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover
|
||||
def plot_ellipse(xCenter, cBest, cMin, eTheta): # pragma: no cover
|
||||
|
||||
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
|
||||
b = cBest / 2.0
|
||||
angle = math.pi / 2.0 - etheta
|
||||
angle = math.pi / 2.0 - eTheta
|
||||
cx = xCenter[0]
|
||||
cy = xCenter[1]
|
||||
|
||||
|
||||
@@ -17,7 +17,7 @@ show_animation = True
|
||||
|
||||
class BidirectionalAStarPlanner:
|
||||
|
||||
def __init__(self, ox, oy, reso, rr):
|
||||
def __init__(self, ox, oy, resolution, rr):
|
||||
"""
|
||||
Initialize grid map for a star planning
|
||||
|
||||
@@ -27,21 +27,24 @@ class BidirectionalAStarPlanner:
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
|
||||
self.reso = reso
|
||||
self.min_x, self.min_y = None, None
|
||||
self.max_x, self.max_y = None, None
|
||||
self.x_width, self.y_width, self.obstacle_map = None, None, None
|
||||
self.resolution = resolution
|
||||
self.rr = rr
|
||||
self.calc_obstacle_map(ox, oy)
|
||||
self.motion = self.get_motion_model()
|
||||
|
||||
class Node:
|
||||
def __init__(self, x, y, cost, pind):
|
||||
def __init__(self, x, y, cost, parent_index):
|
||||
self.x = x # index of grid
|
||||
self.y = y # index of grid
|
||||
self.cost = cost
|
||||
self.pind = pind
|
||||
self.parent_index = parent_index
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(
|
||||
self.cost) + "," + str(self.pind)
|
||||
self.cost) + "," + str(self.parent_index)
|
||||
|
||||
def planning(self, sx, sy, gx, gy):
|
||||
"""
|
||||
@@ -58,18 +61,19 @@ class BidirectionalAStarPlanner:
|
||||
ry: y position list of the final path
|
||||
"""
|
||||
|
||||
nstart = self.Node(self.calc_xyindex(sx, self.minx),
|
||||
self.calc_xyindex(sy, self.miny), 0.0, -1)
|
||||
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
|
||||
self.calc_xyindex(gy, self.miny), 0.0, -1)
|
||||
start_node = self.Node(self.calc_xy_index(sx, self.min_x),
|
||||
self.calc_xy_index(sy, self.min_y), 0.0, -1)
|
||||
goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
|
||||
self.calc_xy_index(gy, self.min_y), 0.0, -1)
|
||||
|
||||
open_set_A, closed_set_A = dict(), dict()
|
||||
open_set_B, closed_set_B = dict(), dict()
|
||||
open_set_A[self.calc_grid_index(nstart)] = nstart
|
||||
open_set_B[self.calc_grid_index(ngoal)] = ngoal
|
||||
open_set_A[self.calc_grid_index(start_node)] = start_node
|
||||
open_set_B[self.calc_grid_index(goal_node)] = goal_node
|
||||
|
||||
current_A = nstart
|
||||
current_B = ngoal
|
||||
current_A = start_node
|
||||
current_B = goal_node
|
||||
meet_point_A, meet_point_B = None, None
|
||||
|
||||
while 1:
|
||||
if len(open_set_A) == 0:
|
||||
@@ -94,22 +98,23 @@ class BidirectionalAStarPlanner:
|
||||
|
||||
# show graph
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(self.calc_grid_position(current_A.x, self.minx),
|
||||
self.calc_grid_position(current_A.y, self.miny), "xc")
|
||||
plt.plot(self.calc_grid_position(current_B.x, self.minx),
|
||||
self.calc_grid_position(current_B.y, self.miny), "xc")
|
||||
plt.plot(self.calc_grid_position(current_A.x, self.min_x),
|
||||
self.calc_grid_position(current_A.y, self.min_y),
|
||||
"xc")
|
||||
plt.plot(self.calc_grid_position(current_B.x, self.min_x),
|
||||
self.calc_grid_position(current_B.y, self.min_y),
|
||||
"xc")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event:
|
||||
[exit(0) if event.key == 'escape'
|
||||
else None])
|
||||
plt.gcf().canvas.mpl_connect(
|
||||
'key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if len(closed_set_A.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
if current_A.x == current_B.x and current_A.y == current_B.y:
|
||||
print("Found goal")
|
||||
meetpointA = current_A
|
||||
meetpointB = current_B
|
||||
meet_point_A = current_A
|
||||
meet_point_B = current_B
|
||||
break
|
||||
|
||||
# Remove the item from the open set
|
||||
@@ -158,7 +163,7 @@ class BidirectionalAStarPlanner:
|
||||
open_set_B[n_ids[1]] = c_nodes[1]
|
||||
|
||||
rx, ry = self.calc_final_bidirectional_path(
|
||||
meetpointA, meetpointB, closed_set_A, closed_set_B)
|
||||
meet_point_A, meet_point_B, closed_set_A, closed_set_B)
|
||||
|
||||
return rx, ry
|
||||
|
||||
@@ -175,16 +180,16 @@ class BidirectionalAStarPlanner:
|
||||
|
||||
return rx, ry
|
||||
|
||||
def calc_final_path(self, ngoal, closedset):
|
||||
def calc_final_path(self, goal_node, closed_set):
|
||||
# generate final course
|
||||
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
|
||||
self.calc_grid_position(ngoal.y, self.miny)]
|
||||
pind = ngoal.parent_index
|
||||
while pind != -1:
|
||||
n = closedset[pind]
|
||||
rx.append(self.calc_grid_position(n.x, self.minx))
|
||||
ry.append(self.calc_grid_position(n.y, self.miny))
|
||||
pind = n.parent_index
|
||||
rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], \
|
||||
[self.calc_grid_position(goal_node.y, self.min_y)]
|
||||
parent_index = goal_node.parent_index
|
||||
while parent_index != -1:
|
||||
n = closed_set[parent_index]
|
||||
rx.append(self.calc_grid_position(n.x, self.min_x))
|
||||
ry.append(self.calc_grid_position(n.y, self.min_y))
|
||||
parent_index = n.parent_index
|
||||
|
||||
return rx, ry
|
||||
|
||||
@@ -210,69 +215,69 @@ class BidirectionalAStarPlanner:
|
||||
f_cost = g_cost + h_cost
|
||||
return f_cost
|
||||
|
||||
def calc_grid_position(self, index, minp):
|
||||
def calc_grid_position(self, index, min_position):
|
||||
"""
|
||||
calc grid position
|
||||
|
||||
:param index:
|
||||
:param minp:
|
||||
:param min_position:
|
||||
:return:
|
||||
"""
|
||||
pos = index * self.reso + minp
|
||||
pos = index * self.resolution + min_position
|
||||
return pos
|
||||
|
||||
def calc_xyindex(self, position, min_pos):
|
||||
return round((position - min_pos) / self.reso)
|
||||
def calc_xy_index(self, position, min_pos):
|
||||
return round((position - min_pos) / self.resolution)
|
||||
|
||||
def calc_grid_index(self, node):
|
||||
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
|
||||
return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
|
||||
|
||||
def verify_node(self, node):
|
||||
px = self.calc_grid_position(node.x, self.minx)
|
||||
py = self.calc_grid_position(node.y, self.miny)
|
||||
px = self.calc_grid_position(node.x, self.min_x)
|
||||
py = self.calc_grid_position(node.y, self.min_y)
|
||||
|
||||
if px < self.minx:
|
||||
if px < self.min_x:
|
||||
return False
|
||||
elif py < self.miny:
|
||||
elif py < self.min_y:
|
||||
return False
|
||||
elif px >= self.maxx:
|
||||
elif px >= self.max_x:
|
||||
return False
|
||||
elif py >= self.maxy:
|
||||
elif py >= self.max_y:
|
||||
return False
|
||||
|
||||
# collision check
|
||||
if self.obmap[node.x][node.y]:
|
||||
if self.obstacle_map[node.x][node.y]:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def calc_obstacle_map(self, ox, oy):
|
||||
|
||||
self.minx = round(min(ox))
|
||||
self.miny = round(min(oy))
|
||||
self.maxx = round(max(ox))
|
||||
self.maxy = round(max(oy))
|
||||
print("min_x:", self.minx)
|
||||
print("min_y:", self.miny)
|
||||
print("max_x:", self.maxx)
|
||||
print("max_y:", self.maxy)
|
||||
self.min_x = round(min(ox))
|
||||
self.min_y = round(min(oy))
|
||||
self.max_x = round(max(ox))
|
||||
self.max_y = round(max(oy))
|
||||
print("min_x:", self.min_x)
|
||||
print("min_y:", self.min_y)
|
||||
print("max_x:", self.max_x)
|
||||
print("max_y:", self.max_y)
|
||||
|
||||
self.xwidth = round((self.maxx - self.minx) / self.reso)
|
||||
self.ywidth = round((self.maxy - self.miny) / self.reso)
|
||||
print("x_width:", self.xwidth)
|
||||
print("y_width:", self.ywidth)
|
||||
self.x_width = round((self.max_x - self.min_x) / self.resolution)
|
||||
self.y_width = round((self.max_y - self.min_y) / self.resolution)
|
||||
print("x_width:", self.x_width)
|
||||
print("y_width:", self.y_width)
|
||||
|
||||
# obstacle map generation
|
||||
self.obmap = [[False for _ in range(self.ywidth)]
|
||||
for _ in range(self.xwidth)]
|
||||
for ix in range(self.xwidth):
|
||||
x = self.calc_grid_position(ix, self.minx)
|
||||
for iy in range(self.ywidth):
|
||||
y = self.calc_grid_position(iy, self.miny)
|
||||
self.obstacle_map = [[False for _ in range(self.y_width)]
|
||||
for _ in range(self.x_width)]
|
||||
for ix in range(self.x_width):
|
||||
x = self.calc_grid_position(ix, self.min_x)
|
||||
for iy in range(self.y_width):
|
||||
y = self.calc_grid_position(iy, self.min_y)
|
||||
for iox, ioy in zip(ox, oy):
|
||||
d = math.hypot(iox - x, ioy - y)
|
||||
if d <= self.rr:
|
||||
self.obmap[ix][iy] = True
|
||||
self.obstacle_map[ix][iy] = True
|
||||
break
|
||||
|
||||
@staticmethod
|
||||
|
||||
@@ -17,7 +17,7 @@ show_animation = True
|
||||
|
||||
class BidirectionalBreadthFirstSearchPlanner:
|
||||
|
||||
def __init__(self, ox, oy, reso, rr):
|
||||
def __init__(self, ox, oy, resolution, rr):
|
||||
"""
|
||||
Initialize grid map for bfs planning
|
||||
|
||||
@@ -27,22 +27,25 @@ class BidirectionalBreadthFirstSearchPlanner:
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
|
||||
self.reso = reso
|
||||
self.min_x, self.min_y = None, None
|
||||
self.max_x, self.max_y = None, None
|
||||
self.x_width, self.y_width, self.obstacle_map = None, None, None
|
||||
self.resolution = resolution
|
||||
self.rr = rr
|
||||
self.calc_obstacle_map(ox, oy)
|
||||
self.motion = self.get_motion_model()
|
||||
|
||||
class Node:
|
||||
def __init__(self, x, y, cost, pind, parent):
|
||||
def __init__(self, x, y, cost, parent_index, parent):
|
||||
self.x = x # index of grid
|
||||
self.y = y # index of grid
|
||||
self.cost = cost
|
||||
self.pind = pind
|
||||
self.parent_index = parent_index
|
||||
self.parent = parent
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(
|
||||
self.cost) + "," + str(self.pind)
|
||||
self.cost) + "," + str(self.parent_index)
|
||||
|
||||
def planning(self, sx, sy, gx, gy):
|
||||
"""
|
||||
@@ -59,15 +62,19 @@ class BidirectionalBreadthFirstSearchPlanner:
|
||||
ry: y position list of the final path
|
||||
"""
|
||||
|
||||
nstart = self.Node(self.calc_xyindex(sx, self.minx),
|
||||
self.calc_xyindex(sy, self.miny), 0.0, -1, None)
|
||||
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
|
||||
self.calc_xyindex(gy, self.miny), 0.0, -1, None)
|
||||
start_node = self.Node(self.calc_xy_index(sx, self.min_x),
|
||||
self.calc_xy_index(sy, self.min_y), 0.0, -1,
|
||||
None)
|
||||
goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
|
||||
self.calc_xy_index(gy, self.min_y), 0.0, -1,
|
||||
None)
|
||||
|
||||
open_set_A, closed_set_A = dict(), dict()
|
||||
open_set_B, closed_set_B = dict(), dict()
|
||||
open_set_B[self.calc_grid_index(ngoal)] = ngoal
|
||||
open_set_A[self.calc_grid_index(nstart)] = nstart
|
||||
open_set_B[self.calc_grid_index(goal_node)] = goal_node
|
||||
open_set_A[self.calc_grid_index(start_node)] = start_node
|
||||
|
||||
meet_point_A, meet_point_B = None, None
|
||||
|
||||
while 1:
|
||||
if len(open_set_A) == 0:
|
||||
@@ -89,28 +96,29 @@ class BidirectionalBreadthFirstSearchPlanner:
|
||||
|
||||
# show graph
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(self.calc_grid_position(current_A.x, self.minx),
|
||||
self.calc_grid_position(current_A.y, self.miny), "xc")
|
||||
plt.plot(self.calc_grid_position(current_B.x, self.minx),
|
||||
self.calc_grid_position(current_B.y, self.miny), "xc")
|
||||
plt.plot(self.calc_grid_position(current_A.x, self.min_x),
|
||||
self.calc_grid_position(current_A.y, self.min_y),
|
||||
"xc")
|
||||
plt.plot(self.calc_grid_position(current_B.x, self.min_x),
|
||||
self.calc_grid_position(current_B.y, self.min_y),
|
||||
"xc")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event:
|
||||
[exit(0) if
|
||||
event.key == 'escape' else None])
|
||||
plt.gcf().canvas.mpl_connect(
|
||||
'key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if len(closed_set_A.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
if c_id_A in closed_set_B:
|
||||
print("Find goal")
|
||||
meetpointA = closed_set_A[c_id_A]
|
||||
meetpointB = closed_set_B[c_id_A]
|
||||
meet_point_A = closed_set_A[c_id_A]
|
||||
meet_point_B = closed_set_B[c_id_A]
|
||||
break
|
||||
|
||||
elif c_id_B in closed_set_A:
|
||||
print("Find goal")
|
||||
meetpointA = closed_set_A[c_id_B]
|
||||
meetpointB = closed_set_B[c_id_B]
|
||||
meet_point_A = closed_set_A[c_id_B]
|
||||
meet_point_B = closed_set_B[c_id_B]
|
||||
break
|
||||
|
||||
# expand_grid search grid based on motion model
|
||||
@@ -137,20 +145,18 @@ class BidirectionalBreadthFirstSearchPlanner:
|
||||
if not self.verify_node(node_B):
|
||||
breakB = True
|
||||
|
||||
if (n_id_A not in closed_set_A) and (n_id_A not in
|
||||
open_set_A) and (not
|
||||
breakA):
|
||||
if (n_id_A not in closed_set_A) and \
|
||||
(n_id_A not in open_set_A) and (not breakA):
|
||||
node_A.parent = current_A
|
||||
open_set_A[n_id_A] = node_A
|
||||
|
||||
if (n_id_B not in closed_set_B) and (n_id_B not in
|
||||
open_set_B) and (not
|
||||
breakB):
|
||||
if (n_id_B not in closed_set_B) and \
|
||||
(n_id_B not in open_set_B) and (not breakB):
|
||||
node_B.parent = current_B
|
||||
open_set_B[n_id_B] = node_B
|
||||
|
||||
rx, ry = self.calc_final_path_bidir(
|
||||
meetpointA, meetpointB, closed_set_A, closed_set_B)
|
||||
meet_point_A, meet_point_B, closed_set_A, closed_set_B)
|
||||
return rx, ry
|
||||
|
||||
# takes both set and meeting nodes and calculate optimal path
|
||||
@@ -166,81 +172,81 @@ class BidirectionalBreadthFirstSearchPlanner:
|
||||
|
||||
return rx, ry
|
||||
|
||||
def calc_final_path(self, ngoal, closedset):
|
||||
def calc_final_path(self, goal_node, closed_set):
|
||||
# generate final course
|
||||
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
|
||||
self.calc_grid_position(ngoal.y, self.miny)]
|
||||
n = closedset[ngoal.parent_index]
|
||||
rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
|
||||
self.calc_grid_position(goal_node.y, self.min_y)]
|
||||
n = closed_set[goal_node.parent_index]
|
||||
while n is not None:
|
||||
rx.append(self.calc_grid_position(n.x, self.minx))
|
||||
ry.append(self.calc_grid_position(n.y, self.miny))
|
||||
rx.append(self.calc_grid_position(n.x, self.min_x))
|
||||
ry.append(self.calc_grid_position(n.y, self.min_y))
|
||||
n = n.parent
|
||||
|
||||
return rx, ry
|
||||
|
||||
def calc_grid_position(self, index, minp):
|
||||
def calc_grid_position(self, index, min_position):
|
||||
"""
|
||||
calc grid position
|
||||
|
||||
:param index:
|
||||
:param minp:
|
||||
:param min_position:
|
||||
:return:
|
||||
"""
|
||||
pos = index * self.reso + minp
|
||||
pos = index * self.resolution + min_position
|
||||
return pos
|
||||
|
||||
def calc_xyindex(self, position, min_pos):
|
||||
return round((position - min_pos) / self.reso)
|
||||
def calc_xy_index(self, position, min_pos):
|
||||
return round((position - min_pos) / self.resolution)
|
||||
|
||||
def calc_grid_index(self, node):
|
||||
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
|
||||
return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
|
||||
|
||||
def verify_node(self, node):
|
||||
px = self.calc_grid_position(node.x, self.minx)
|
||||
py = self.calc_grid_position(node.y, self.miny)
|
||||
px = self.calc_grid_position(node.x, self.min_x)
|
||||
py = self.calc_grid_position(node.y, self.min_y)
|
||||
|
||||
if px < self.minx:
|
||||
if px < self.min_x:
|
||||
return False
|
||||
elif py < self.miny:
|
||||
elif py < self.min_y:
|
||||
return False
|
||||
elif px >= self.maxx:
|
||||
elif px >= self.max_x:
|
||||
return False
|
||||
elif py >= self.maxy:
|
||||
elif py >= self.max_y:
|
||||
return False
|
||||
|
||||
# collision check
|
||||
if self.obmap[node.x][node.y]:
|
||||
if self.obstacle_map[node.x][node.y]:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def calc_obstacle_map(self, ox, oy):
|
||||
|
||||
self.minx = round(min(ox))
|
||||
self.miny = round(min(oy))
|
||||
self.maxx = round(max(ox))
|
||||
self.maxy = round(max(oy))
|
||||
print("min_x:", self.minx)
|
||||
print("min_y:", self.miny)
|
||||
print("max_x:", self.maxx)
|
||||
print("max_y:", self.maxy)
|
||||
self.min_x = round(min(ox))
|
||||
self.min_y = round(min(oy))
|
||||
self.max_x = round(max(ox))
|
||||
self.max_y = round(max(oy))
|
||||
print("min_x:", self.min_x)
|
||||
print("min_y:", self.min_y)
|
||||
print("max_x:", self.max_x)
|
||||
print("max_y:", self.max_y)
|
||||
|
||||
self.xwidth = round((self.maxx - self.minx) / self.reso)
|
||||
self.ywidth = round((self.maxy - self.miny) / self.reso)
|
||||
print("x_width:", self.xwidth)
|
||||
print("y_width:", self.ywidth)
|
||||
self.x_width = round((self.max_x - self.min_x) / self.resolution)
|
||||
self.y_width = round((self.max_y - self.min_y) / self.resolution)
|
||||
print("x_width:", self.x_width)
|
||||
print("y_width:", self.y_width)
|
||||
|
||||
# obstacle map generation
|
||||
self.obmap = [[False for _ in range(self.ywidth)]
|
||||
for _ in range(self.xwidth)]
|
||||
for ix in range(self.xwidth):
|
||||
x = self.calc_grid_position(ix, self.minx)
|
||||
for iy in range(self.ywidth):
|
||||
y = self.calc_grid_position(iy, self.miny)
|
||||
self.obstacle_map = [[False for _ in range(self.y_width)]
|
||||
for _ in range(self.x_width)]
|
||||
for ix in range(self.x_width):
|
||||
x = self.calc_grid_position(ix, self.min_x)
|
||||
for iy in range(self.y_width):
|
||||
y = self.calc_grid_position(iy, self.min_y)
|
||||
for iox, ioy in zip(ox, oy):
|
||||
d = math.hypot(iox - x, ioy - y)
|
||||
if d <= self.rr:
|
||||
self.obmap[ix][iy] = True
|
||||
self.obstacle_map[ix][iy] = True
|
||||
break
|
||||
|
||||
@staticmethod
|
||||
|
||||
@@ -125,6 +125,7 @@ class DijkstraSearch:
|
||||
if self.is_same_node_with_xy(node_x_list[i], node_y_list[i],
|
||||
target_node):
|
||||
return i
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def is_same_node_with_xy(node_x, node_y, node_b):
|
||||
|
||||
Reference in New Issue
Block a user