diff --git a/.gitignore b/.gitignore index 272dc63b6d0..c971b8f9c57 100644 --- a/.gitignore +++ b/.gitignore @@ -70,3 +70,4 @@ target/ .ipynb_checkpoints matplotrecorder/* +.vscode/settings.json diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index 62da6d761be..ee88b3f9f1d 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -33,7 +33,7 @@ def __init__(self, x, y, cost, pind): self.x = x # index of grid self.y = y # index of grid self.cost = cost - self.pind = pind + self.pind = pind # index of previous Node def __str__(self): return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) @@ -88,10 +88,10 @@ def planning(self, sx, sy, gx, gy): closedset[c_id] = current # expand search grid based on motion model - for i, _ in enumerate(self.motion): - node = self.Node(current.x + self.motion[i][0], - current.y + self.motion[i][1], - current.cost + self.motion[i][2], c_id) + for move_x, move_y, move_cost in self.motion: + node = self.Node(current.x + move_x, + current.y + move_y, + current.cost + move_cost, c_id) n_id = self.calc_index(node) if n_id in closedset: @@ -124,11 +124,6 @@ def calc_final_path(self, ngoal, closedset): return rx, ry - def calc_heuristic(self, n1, n2): - w = 1.0 # weight of heuristic - d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) - return d - def calc_position(self, index, minp): pos = index*self.reso+minp return pos @@ -242,6 +237,9 @@ def main(): dijkstra = Dijkstra(ox, oy, grid_size, robot_radius) rx, ry = dijkstra.planning(sx, sy, gx, gy) + print(rx) + print(ry) + if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") plt.show()