This commit is contained in:
Atsushi Sakai
2020-02-29 14:59:40 +09:00
parent ee423e8b94
commit ac1a903ca7
4 changed files with 19 additions and 19 deletions

View File

@@ -24,11 +24,6 @@ class Dijkstra:
rr: robot radius[m]
"""
self.resolution = resolution
self.robot_radius = robot_radius
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
self.min_x = None
self.min_y = None
self.max_x = None
@@ -37,6 +32,11 @@ class Dijkstra:
self.y_width = None
self.obstacle_map = None
self.resolution = resolution
self.robot_radius = robot_radius
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
class Node:
def __init__(self, x, y, cost, parent):
self.x = x # index of grid
@@ -151,11 +151,11 @@ class Dijkstra:
if px < self.min_x:
return False
elif py < self.min_y:
if py < self.min_y:
return False
elif px >= self.max_x:
if px >= self.max_x:
return False
elif py >= self.max_y:
if py >= self.max_y:
return False
if self.obstacle_map[node.x][node.y]:
@@ -169,15 +169,15 @@ class Dijkstra:
self.min_y = round(min(oy))
self.max_x = round(max(ox))
self.max_y = round(max(oy))
print("minx:", self.min_x)
print("miny:", self.min_y)
print("maxx:", self.max_x)
print("maxy:", self.max_y)
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.x_width = round((self.max_x - self.min_x) / self.resolution)
self.y_width = round((self.max_y - self.min_y) / self.resolution)
print("xwidth:", self.x_width)
print("ywidth:", self.y_width)
print("x_width:", self.x_width)
print("y_width:", self.y_width)
# obstacle map generation
self.obstacle_map = [[False for _ in range(self.y_width)]

View File

@@ -18,10 +18,9 @@ class Geometry:
float(q.x - p.x) * (r.y - q.y))
if val > 0:
return 1
elif val < 0:
if val < 0:
return 2
else:
return 0
return 0
# Find the 4 orientations required for
# the general and special cases

View File

@@ -204,7 +204,8 @@ def main():
if show_animation: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
[ob.plot() for ob in obstacles]
for ob in obstacles:
ob.plot()
plt.pause(1.0)
rx, ry = VisibilityRoadMap(robot_radius, do_plot=show_animation).planning(

View File

@@ -121,7 +121,7 @@ class DijkstraSearch:
return False
def find_id(self, node_x_list, node_y_list, target_node):
for i in range(len(node_x_list)):
for i, _ in enumerate(node_x_list):
if self.is_same_node_with_xy(node_x_list[i], node_y_list[i],
target_node):
return i