代码只是A*获取了从起点到达目的地的最短路径，但是对何时进行横向的偏移未做约束。
1 # -*- coding: utf-8 -*- 2 """ 3 Spyder Editor 4 5 This is a temporary script file. 6 """ 7 8 import numpy as np 9 import matplotlib.pyplot as plt 10 import math 11 import queue 12 import time 13 import copy 14 15 detaWidth = 0.3 16 totalWidth = 6.0 #road width 17 detaLength = 3.0 18 totalLength = 30.0 # road predict length 19 vehicleHarfWidth = 0.8 20 21 # s,l half width,half height /m 22 obstacle = np.array([[10,5,0.3,0.3], 23 [16,1.5,0.6,0.3]]) 24 25 numberOfL = np.int(totalWidth /detaWidth) 26 numberOfS = np.int(totalLength /detaLength) 27 28 class StrPosition: 29 def __init__(self,s,l): 30 self.s = s 31 self.l = l 32 33 34 class StrReachablePosition: 35 def __init__(self,parentNode,reachNode): 36 self.parentNode = parentNode 37 self.reachNode = reachNode 38 self.f =999 39 self.g =999 40 self.h =999 41 42 43 44 def findInQueue(closeMap,node): 45 ikey = node.reachNode.l * numberOfS + node.reachNode.s 46 if closeMap.get(ikey,0) == 0: 47 return False 48 return True 49 50 51 def refreshMap(node,openList): 52 ikey = node.reachNode.l * numberOfS + node.reachNode.s 53 openList[ikey] = node 54 return openList 55 56 def updateNode(node,openList,closeMap): 57 #print("updateNode-------------",node.reachNode.l,node.reachNode.s) 58 ikey = node.reachNode.l * numberOfS + node.reachNode.s 59 if openList.get(ikey,0) != 0: 60 nodet = openList[ikey] 61 if(nodet.g > node.g): 62 openList[ikey] = node 63 else: 64 if findInQueue(closeMap,node) == False: 65 openList[ikey] = node 66 return copy.deepcopy(openList) 67 68 def delMap(node,openList): 69 ikey = node.reachNode.l * numberOfS + node.reachNode.s 70 if openList.get(ikey,0) == 0 or ikey == 0: 71 return openList 72 del openList[ikey] 73 return openList 74 75 76 def environmentShow(environment,startIndex,endIndex): 77 78 shape = environment.shape 79 row = shape[0] 80 col= shape[1] 81 for rowi in range(startIndex,endIndex): 82 for coli in range(col): 83 x = np.array([0,col-1]) 84 y = np.array([rowi,rowi]) 85 x1 = np.array([coli,coli]) 86 y1 = np.array([0,(row -1)]) 87 plt.plot(x,y,'b-') 88 plt.plot(x1,y1,'b-') 89 if environment[rowi][coli] >0.1: 90 plt.plot(coli,rowi,'o','r') 91 92 def createObstacle(environment,obstacle,startIndex,endIndex): 93 94 eshape = environment.shape 95 erow = eshape[0] 96 ecol= eshape[1] 97 98 oshape = obstacle.shape 99 for orowi in range(oshape[0]): 100 101 start_l = math.floor( (obstacle[orowi][1]-obstacle[orowi][2]) / detaWidth) - startIndexOfL 102 end_l = math.ceil((obstacle[orowi][1]+obstacle[orowi][2]) / detaWidth) + startIndexOfL 103 104 start_s = math.floor(( obstacle[orowi][0] -obstacle[orowi][3])/detaLength) 105 end_s = math.ceil(( obstacle[orowi][0] + obstacle[orowi][3])/detaLength) 106 print("obstacle s range",start_s,end_s) 107 for obsl_i in range(start_l,end_l+1): 108 for obss_i in range(start_s,end_s+1): 109 if obsl_i>=0 and obsl_i < erow and obss_i >= 0 and obss_i< ecol: 110 environment[obsl_i][obss_i] = 1.0 111 112 113 def createStopPosition(x,startIndexaOfL,endIndexOfL): 114 stopPosition = np.zeros((endIndexOfL - startIndexOfL,2)) 115 for index in range(endIndexOfL-startIndexaOfL): 116 stopPosition[index][0] = x 117 stopPosition[index][1] = index 118 return stopPosition 119 120 def getHCost(startPosition,endPosition): 121 normal = math.hypot( (startPosition.s - endPosition.s)*detaLength,(startPosition.l -endPosition.l)*detaWidth) 122 #lcost = (startPosition.l -endPosition.l)*detaWidth * (startPosition.l -endPosition.l)*detaWidth *0.2 123 lcost =0 124 return normal + lcost 125 126 def getGCost(node,endPosition): 127 return node.g + math.hypot((node.reachNode.s - endPosition.s)*detaLength,(node.reachNode.l -endPosition.l)*detaWidth) 128 129 def getFCost(node): 130 return node.g+node.h 131 132 def reachPoints(openList,environment,lastNode,stopPosition,closeMap, startIndexOfL,endIndexOfL): 133 print("reachPoints-------------") 134 parentNode = lastNode.reachNode 135 nexts = parentNode.s + 1 136 reachNode = StrPosition(nexts,0) 137 node = StrReachablePosition(parentNode,reachNode) 138 139 shape = environment.shape 140 node.parentNode = parentNode 141 print("nexts,s of env",nexts , shape[1]) 142 if nexts < shape[1]: 143 if parentNode.l + 1 <= endIndexOfL: 144 if environment[parentNode.l + 1][nexts] < 0.1: 145 reachNode.l =parentNode.l +1 146 node.reachNode = reachNode 147 node.g = getGCost(lastNode,reachNode) 148 node.h = getHCost(reachNode,stopPosition) 149 node.f = getFCost(node) 150 openList = updateNode(node,openList,closeMap) 151 152 153 if environment[parentNode.l][nexts] < 0.1: 154 reachNode.l =parentNode.l 155 node.reachNode = reachNode 156 node.g = getGCost(lastNode,reachNode) 157 node.h = getHCost(reachNode,stopPosition) 158 node.f = getFCost(node) 159 openList = updateNode(node,openList,closeMap) 160 161 if parentNode.l -1 >= startIndexOfL: 162 if environment[parentNode.l -1 ][nexts] < 0.1: 163 reachNode.l =parentNode.l -1 164 node.reachNode = reachNode 165 node.g = getGCost(lastNode,reachNode) 166 node.h = getHCost(reachNode,stopPosition) 167 node.f = getFCost(node) 168 openList = updateNode(node,openList,closeMap) 169 return openList 170 171 def freshCloseList(node,openList,closeList,closeMap): 172 print("freshCloseList-------------") 173 ikey = node.reachNode.l * numberOfS + node.reachNode.s 174 closeList.put(node) 175 closeMap[ikey] =node.parentNode.l * numberOfS + node.parentNode.s 176 return delMap(node,openList),closeList,closeMap 177 178 179 def getMinFFromOpenList(openList): 180 print("getMinFFromOpenList-------------") 181 #find min node 182 start = StrPosition(0,0) 183 end = StrPosition(0,0) 184 noden = StrReachablePosition(start,end) 185 if len(openList) == 0: 186 return noden,False 187 minf =999999 188 for nodet in openList.items(): 189 if minf >nodet[1].f: 190 minf = nodet[1].f 191 noden = nodet[1] 192 193 print("min",minf,noden.reachNode.s,noden.reachNode.l) 194 return noden,True 195 196 197 198 def AStart(environment,startPosition,stopPosition,startIndexOfL,endIndexOfL): 199 print("AStart-------------") 200 201 #put startnode to queue 202 node = StrReachablePosition(startPosition,startPosition) 203 node.g = 0 204 node.h = getGCost(node,startPosition) 205 node.f = getFCost(node) 206 openList = dict() 207 openList = refreshMap(node,openList) 208 print("dict len",len(openList)) 209 print("reachPosition",node.reachNode.s,node.reachNode.l) 210 time.sleep(1) 211 closeList = queue.Queue() 212 closeMap = dict() 213 #search 214 while node.reachNode.s != stopPosition.s or \ 215 node.reachNode.l != stopPosition.l and len(openList)!=0: 216 # update reachabe node 217 #print("node.reachNode.s,stopPosition.s,node.reachNode.l,stopPosition.l",node.reachNode.s,stopPosition.s,node.reachNode.l,stopPosition.l) 218 openList = reachPoints(openList,environment,node,stopPosition,closeMap,startIndexOfL,endIndexOfL) 219 #time.sleep(1) 220 ##------------- 221 for nodet in openList.items(): 222 print("openlist",nodet[0],nodet[1].reachNode.s,nodet[1].reachNode.l,nodet[1].f) 223 ##---------------- 224 node,flag = getMinFFromOpenList(openList) 225 if flag == False: 226 print("openlist null") 227 return closeList,closeMap,False 228 else: 229 openList,closeList,closeMap = freshCloseList(node,openList,closeList,closeMap) 230 ##------------- 231 #print("closemaplen",len(closeMap)) 232 #for nodet2 in closeMap.items(): 233 # print("closemap",nodet2[0],nodet2[1]) 234 ##---------------- 235 236 if node.reachNode.s == stopPosition.s and node.reachNode.l == stopPosition.l: 237 return closeList,closeMap,True 238 print("openlist null",len(openList)) 239 return closeList, closeMap,False 240 241 def showTrajack(closeMap,startPosition,stopPosition): 242 startkey = startPosition.l * numberOfS + startPosition.s 243 endkey = stopPosition.l * numberOfS + stopPosition.s 244 245 ikey = closeMap[endkey] 246 slist = list() 247 llist = list() 248 249 slist.append(stopPosition.s) 250 llist.append(stopPosition.l) 251 while ikey != startkey: 252 s = np.int(ikey % numberOfS) 253 l = np.int((ikey - s) / numberOfS) 254 ikey = closeMap[ikey] 255 slist.append(s) 256 llist.append(l) 257 258 slist.append(startPosition.s) 259 llist.append(startPosition.l) 260 plt.plot(slist,llist,'*') 261 262 263 264 265 def showCloseList(closeList): 266 while not closeList.empty(): 267 node = closeList.get() 268 plt.plot(node.reachNode.s,node.reachNode.l,'r*') 269 270 271 272 273 if __name__ == '__main__': 274 275 # vehicle to road region of l 276 startIndexOfL = np.int(vehicleHarfWidth / detaWidth) 277 endIndexOfL = numberOfL - np.int(vehicleHarfWidth / detaWidth) 278 279 environment = np.zeros((numberOfL,numberOfS)) 280 createObstacle(environment,obstacle,startIndexOfL,endIndexOfL) 281 environmentShow(environment,startIndexOfL,endIndexOfL) 282 283 284 285 #simple end a*-------------------------------------------- 286 '''startPosition = StrPosition(0,9) 287 stopPosition = StrPosition(8,7) 288 closeList,closeMap,Flag = AStart(environment,startPosition,stopPosition,startIndexOfL,endIndexOfL) 289 290 291 if Flag == True: 292 showTrajack(closeMap,startPosition,stopPosition) 293 else: 294 print("no route-----------------") 295 showCloseList(closeList) 296 ''' 297 298 #multi end a* 299 300 stopPosition = createStopPosition(numberOfS-1,startIndexOfL,endIndexOfL) 301 startPosition = StrPosition(0,9) 302 303 shapeOfEnd = stopPosition.shape 304 for i in range(0,shapeOfEnd[0]): 305 stop = StrPosition(stopPosition[i][0],stopPosition[i][1]) 306 closeList,closeMap,Flag = AStart(environment,startPosition,stop,startIndexOfL,endIndexOfL) 307 if Flag == True: 308 showTrajack(closeMap,startPosition,stop) 309 else: 310 print("no route-----------------") 311 #showCloseList(closeList) 312 313 314 315 316 317 318 319 320 321 322
