mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-02-10 12:24:58 -05:00
fix scanning error (#339)
This commit is contained in:
Atsushi Sakai
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user