mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
114 lines
2.2 KiB
Python
114 lines
2.2 KiB
Python
"""
|
|
|
|
Dijkstra grid based planning
|
|
|
|
author: Atsushi Sakai
|
|
"""
|
|
|
|
import matplotlib.pyplot as plt
|
|
import math
|
|
|
|
|
|
class Node:
|
|
|
|
def __init__(self, x, y, cost, pind):
|
|
self.x = x
|
|
self.y = y
|
|
self.cost = cost
|
|
self.pind = pind
|
|
|
|
|
|
def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
|
"""
|
|
gx: goal x position [m]
|
|
gx: goal x position [m]
|
|
ox: x position list of Obstacles [m]
|
|
oy: y position list of Obstacles [m]
|
|
reso: grid resolution [m]
|
|
rr: robot radius[m]
|
|
"""
|
|
|
|
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
|
|
ox = [iox / reso for iox in ox]
|
|
oy = [ioy / reso for ioy in oy]
|
|
|
|
motion = get_motion_model()
|
|
# print(motion)
|
|
|
|
opened, closed = [], []
|
|
opened.append(ngoal)
|
|
|
|
while True:
|
|
if len(opened) == 0:
|
|
print("Finish Search")
|
|
break
|
|
|
|
# expand search grid based on motion model
|
|
for i in range(len(motion)):
|
|
pass
|
|
|
|
rx, ry = [], []
|
|
|
|
return rx, ry
|
|
|
|
|
|
def get_motion_model():
|
|
# dx, dy, cost
|
|
motion = [[1, 0, 1],
|
|
[0, 1, 1],
|
|
[-1, 0, 1],
|
|
[0, -1, 1],
|
|
[-1, -1, math.sqrt(2)],
|
|
[-1, 1, math.sqrt(2)],
|
|
[1, -1, math.sqrt(2)],
|
|
[1, 1, math.sqrt(2)]]
|
|
|
|
return motion
|
|
|
|
|
|
def main():
|
|
print(__file__ + " start!!")
|
|
|
|
# start and goal position
|
|
sx = 10.0 # [m]
|
|
sy = 10.0 # [m]
|
|
gx = 50.0 # [m]
|
|
gy = 50.0 # [m]
|
|
grid_size = 1.0 # [m]
|
|
robot_size = 1.0 # [m]
|
|
|
|
ox = []
|
|
oy = []
|
|
|
|
for i in range(60):
|
|
ox.append(i)
|
|
oy.append(0.0)
|
|
for i in range(60):
|
|
ox.append(60.0)
|
|
oy.append(i)
|
|
for i in range(60):
|
|
ox.append(i)
|
|
oy.append(60.0)
|
|
for i in range(60):
|
|
ox.append(0.0)
|
|
oy.append(i)
|
|
for i in range(40):
|
|
ox.append(20.0)
|
|
oy.append(i)
|
|
for i in range(40):
|
|
ox.append(40.0)
|
|
oy.append(60.0 - i)
|
|
|
|
rx, ry = dijkstra_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
|
|
|
|
plt.plot(ox, oy, ".k")
|
|
plt.plot(sx, sy, "xr")
|
|
plt.plot(gx, gy, "xb")
|
|
plt.grid(True)
|
|
plt.axis("equal")
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|