tring to implement

PathPlanning/Dijkstra/dijkstra.py

@@ -5,58 +5,108 @@ Dijkstra grid based planning
 author: Atsushi Sakai
 """
 
+import matplotlib.pyplot as plt
+import math
+
 
 class Node:
 
-    def __init__(self):
-        self.x = -1
-        self.y = -1
-        self.cost = 0.0
-        self.pind = -1
+    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 = 50.0  # [m]
-    sy = 450.0  # [m]
-    gx = -50.0  # [m]
-    gy = 560.0  # [m]
+    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 0:
-    #  60
-    #  push!(ox, Float64(i))
-    #  push!(oy, 0.0)
-    #  end
-    #  for i in 0:
-    #  60
-    #  push!(ox, 60.0)
-    #  push!(oy, Float64(i))
-    #  end
-    #  for i in 0:
-    #  60
-    #  push!(ox, Float64(i))
-    #  push!(oy, 60.0)
-    #  end
-    #  for i in 0:
-    #  60
-    #  push!(ox, 0.0)
-    #  push!(oy, Float64(i))
-    #  end
-    #  for i in 0:
-    #  40
-    #  push!(ox, 20.0)
-    #  push!(oy, Float64(i))
-    #  end
-    #  for i in 0:
-    #  40
-    #  push!(ox, 40.0)
-    #  push!(oy, 60.0 - Float64(i))
-    #  end
+    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__':