github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

add a star test and dijkstra test

Browse Source
This commit is contained in:
Atsushi Sakai
2018-02-14 09:16:22 -08:00
parent da82628b33
commit 041ad9fb3c
4 changed files with 77 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
PathPlanning/Dijkstra/dijkstra.py
View File

@@ -1,13 +1,13 @@
"""
Dijkstra grid based planning
author: Atsushi Sakai
author: Atsushi Sakai(@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import math
from matplotrecorder import matplotrecorder
matplotrecorder.donothing = True
show_animation = True
class Node:
@@ -50,10 +50,10 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
# print("current", current)
# show graph
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
matplotrecorder.save_frame()
if show_animation:
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
@@ -85,6 +85,12 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
else:
openset[n_id] = node
rx, ry = calc_fianl_path(ngoal, closedset, reso)
return rx, ry
def calc_fianl_path(ngoal, closedset, reso):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
@@ -197,21 +203,18 @@ def main():
ox.append(40.0)
oy.append(60.0 - i)
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
if show_animation:
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry = dijkstra_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
plt.plot(rx, ry, "-r")
for i in range(10):
matplotrecorder.save_frame()
plt.show()
matplotrecorder.save_movie("animation.gif", 0.1)
if show_animation:
plt.plot(rx, ry, "-r")
plt.show()
if __name__ == '__main__':