astar.py 4.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192
  1. import math
  2. import numpy as np
  3. import matplotlib.pyplot as plt
  4. from mpl_toolkits.mplot3d import Axes3D
  5. from scipy import signal
  6. class Plane:
  7. def __init__(self, position, orientation):
  8. self.position = position
  9. self.orientation = orientation
  10. class Node:
  11. def __init__(self, x, y, z, cost, pind):
  12. self.x = x
  13. self.y = y
  14. self.z = z
  15. self.cost = cost
  16. self.pind = pind
  17. def __str__(self):
  18. return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
  19. def calc_final_path(ngoal, closedset, reso):
  20. # generate final course
  21. rx, ry, rz = [ngoal.x * reso], [ngoal.y * reso], [ngoal.z * reso]
  22. pind = ngoal.pind
  23. while pind != -1:
  24. n = closedset[pind]
  25. rx.append(n.x * reso)
  26. ry.append(n.y * reso)
  27. rz.append(n.z * reso)
  28. pind = n.pind
  29. return rx, ry, rz
  30. def calc_heuristic(n1, n2):
  31. w = 1.0 # weight of heuristic
  32. d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2 + (n1.z-n2.z)**2)
  33. return d
  34. def calc_index(node, xwidth, xmin, ymin):
  35. return (node.y - ymin) * xwidth + (node.x - xmin)
  36. def get_motion_model():
  37. # Always go forward one step (10 meter) so cost is
  38. # dl distance lateral
  39. # dz vertical distance
  40. # cost
  41. motion = [
  42. [0, 0, 1],
  43. [1, 0, math.sqrt(2)],
  44. [-1, 0,math.sqrt(2)],
  45. # go up
  46. [0, 1, math.sqrt(2)],
  47. [1, 1, math.sqrt(math.sqrt(2)+1)],
  48. [-1, 1, math.sqrt(math.sqrt(2)+1)],
  49. # go down
  50. [0, -1, math.sqrt(2)],
  51. [1, -1, math.sqrt(math.sqrt(2)+1)],
  52. [-1,-1, math.sqrt(math.sqrt(2)+1)],
  53. ]
  54. return motion
  55. def calc_obstacle_map(ox, oy, oz, reso, rr):
  56. minx = round(min(ox))
  57. miny = round(min(oy))
  58. maxx = round(max(ox))
  59. maxy = round(max(oy))
  60. maxz = round(max(oz))
  61. minz = round(min(oz))
  62. # print("minx:", minx)
  63. # print("miny:", miny)
  64. # print("maxx:", maxx)
  65. # print("maxy:", maxy)
  66. xwidth = round(maxx - minx)
  67. ywidth = round(maxy - miny)
  68. zwidth = int(round(maxz - minz))
  69. print(zwidth)
  70. # print("xwidth:", xwidth)
  71. # print("ywidth:", ywidth)
  72. # obstacle map generation
  73. obmap = [[[False for i in range(ywidth)] for i in range(xwidth)] for i in range(zwidth)]
  74. print(obmap)
  75. for ix in range(xwidth):
  76. x = ix + minx
  77. for iy in range(ywidth):
  78. y = iy + miny
  79. for iz in range(zwidth):
  80. z = iz + minz
  81. for iox, ioy, ioz in zip(ox, oy, oz):
  82. d = math.sqrt((iox - x)**2 + (ioy - y)**2 + (ioz-z)**2)
  83. if d <= rr / reso:
  84. try:
  85. obmap[ix][iy][iz] = True
  86. break
  87. except Exception:
  88. pass
  89. return obmap, minx, miny, maxx, maxy, xwidth, ywidth
  90. def manathan(a, b):
  91. return 1
  92. def astar(sx, sy, sz, gx, gy, gz, ox, oy, oz, rr):
  93. orientation_norm = math.sqrt((gx-sx)**2+(gy-sy)**2)
  94. orientation = ((gx-sx)/orientation_norm, (gy-sy)/orientation_norm)
  95. print(orientation)
  96. motion = get_motion_model()
  97. # Start at sx sy sz
  98. pos = [sx, sy, sz]
  99. plane = Plane(pos, orientation)
  100. current_node = Node(sx, sy, sz, 0, None)
  101. # Calculate next step
  102. while 1:
  103. g = 0
  104. # Cost of step starts at infinity
  105. cost = 1000
  106. # Calculate G and H for each of the 9 option
  107. for i in range(0, 9) :
  108. # Get the manathan for the next 9 nodes
  109. H = manathan(sx, gx)
  110. g = g + motion[i][2]
  111. print(g, H)
  112. return rx, ry, rz
  113. show_animation = True
  114. goal = (22, 30, 0)
  115. start = (2, 10, 0)
  116. ox = [10]
  117. oy = [20]
  118. oz = [0]
  119. # Set up grid and test data
  120. nx, ny = 64, 64
  121. x = range(nx)
  122. y = range(ny)
  123. dem1 = np.random.rand(nx,ny)
  124. sizex = 12
  125. sizey = 12
  126. x, y = np.mgrid[-sizex:sizex+1, -sizey:sizey+1]
  127. g = np.exp(-0.5*(x**2/float(sizex)+y**2/float(sizey)))
  128. filter = g/g.sum()
  129. demSmooth = signal.convolve(dem1,filter,mode='valid')
  130. # rescale so it lies between 0 and 1
  131. demSmooth = (demSmooth - demSmooth.min())/(demSmooth.max() - demSmooth.min())*10
  132. print(demSmooth)
  133. print(demSmooth[0,0])
  134. plt.imshow(demSmooth)
  135. plt.plot(start[0], start[1],'bo' )
  136. plt.plot(goal[0], goal[1],'bo' )
  137. # Add obstacles to list
  138. ox, oy, oz = [], [], []
  139. #for x in range(0, len(demSmooth)):
  140. # for y in range(0, len(demSmooth)):
  141. # ox.append(x)
  142. # oy.append(y)
  143. # oz.append(demSmooth[x, y])
  144. #
  145. print(oz)
  146. from mpl_toolkits.mplot3d import Axes3D
  147. fig = plt.figure()
  148. #ax = fig.add_subplot(111, projection='3d')
  149. rx, ry, rz = astar(start[0], start[1], start[2], goal[0], goal[1], goal[2], ox, oy, oz, 1)
  150. if show_animation: # pragma: no cover
  151. #Axes3D.plot(rx, ry,rz)
  152. print(rz)
  153. print(ry)
  154. plt.show()
  155. def firstpass():
  156. """
  157. Gets height across a 150 meters wide section between the start and the goal.
  158. """
  159. return None