ASttar 路径规划之初级

代码只是A*获取了从起点到达目的地的最短路径，但是对何时进行横向的偏移未做约束。

# -*- coding: utf-8 -*-
"""
Spyder Editor

This is a temporary script file.
"""

import numpy as  np
import matplotlib.pyplot as plt
import math
import queue
import time
import copy

detaWidth = 0.3
totalWidth = 6.0   #road width
detaLength = 3.0   
totalLength = 30.0 # road predict length
vehicleHarfWidth = 0.8

# s,l half width,half height /m
obstacle = np.array([[10,5,0.3,0.3],
                     [16,1.5,0.6,0.3]])

numberOfL = np.int(totalWidth /detaWidth)
numberOfS = np.int(totalLength /detaLength)

class StrPosition:
    def __init__(self,s,l):
        self.s = s
        self.l = l


class StrReachablePosition:
    def __init__(self,parentNode,reachNode):
        self.parentNode = parentNode
        self.reachNode = reachNode
        self.f =999
        self.g =999
        self.h =999
        
        
        
def findInQueue(closeMap,node):
     ikey = node.reachNode.l * numberOfS + node.reachNode.s
     if closeMap.get(ikey,0) == 0:
         return False
     return True


def refreshMap(node,openList):
      ikey = node.reachNode.l * numberOfS + node.reachNode.s
      openList[ikey] = node
      return openList
      
def updateNode(node,openList,closeMap):
    #print("updateNode-------------",node.reachNode.l,node.reachNode.s)
    ikey = node.reachNode.l * numberOfS + node.reachNode.s
    if openList.get(ikey,0) != 0:
        nodet = openList[ikey]
        if(nodet.g > node.g):
            openList[ikey] = node
    else:
         if findInQueue(closeMap,node) == False:
             openList[ikey] = node
    return copy.deepcopy(openList)

def delMap(node,openList):
    ikey = node.reachNode.l * numberOfS + node.reachNode.s
    if openList.get(ikey,0) == 0  or ikey == 0:
        return openList
    del openList[ikey] 
    return openList


def environmentShow(environment,startIndex,endIndex):

    shape = environment.shape
    row = shape[0]
    col= shape[1]
    for rowi in range(startIndex,endIndex):
        for coli in range(col):
          x = np.array([0,col-1])
          y = np.array([rowi,rowi])
          x1 = np.array([coli,coli])
          y1 = np.array([0,(row -1)])
          plt.plot(x,y,'b-')
          plt.plot(x1,y1,'b-')
          if environment[rowi][coli] >0.1:
              plt.plot(coli,rowi,'o','r')

def createObstacle(environment,obstacle,startIndex,endIndex):
    
    eshape = environment.shape
    erow = eshape[0]
    ecol= eshape[1]
    
    oshape = obstacle.shape
    for orowi in  range(oshape[0]):

        start_l = math.floor( (obstacle[orowi][1]-obstacle[orowi][2]) / detaWidth) - startIndexOfL
        end_l = math.ceil((obstacle[orowi][1]+obstacle[orowi][2]) / detaWidth) + startIndexOfL
        
        start_s = math.floor(( obstacle[orowi][0] -obstacle[orowi][3])/detaLength)
        end_s = math.ceil(( obstacle[orowi][0] + obstacle[orowi][3])/detaLength)
        print("obstacle s range",start_s,end_s)
        for obsl_i in range(start_l,end_l+1):
            for obss_i in range(start_s,end_s+1):
                if obsl_i>=0 and obsl_i < erow and obss_i >= 0 and obss_i< ecol:
                    environment[obsl_i][obss_i] = 1.0
                    
            
def createStopPosition(x,startIndexaOfL,endIndexOfL):
    stopPosition = np.zeros((endIndexOfL - startIndexOfL,2))
    for index in range(endIndexOfL-startIndexaOfL):
        stopPosition[index][0] = x
        stopPosition[index][1] = index
    return stopPosition

def getHCost(startPosition,endPosition):
    normal = math.hypot( (startPosition.s - endPosition.s)*detaLength,(startPosition.l -endPosition.l)*detaWidth)
    #lcost = (startPosition.l -endPosition.l)*detaWidth * (startPosition.l -endPosition.l)*detaWidth *0.2
    lcost =0
    return normal + lcost

def getGCost(node,endPosition):
    return node.g + math.hypot((node.reachNode.s - endPosition.s)*detaLength,(node.reachNode.l -endPosition.l)*detaWidth)

def getFCost(node):
    return node.g+node.h

def reachPoints(openList,environment,lastNode,stopPosition,closeMap, startIndexOfL,endIndexOfL):
    print("reachPoints-------------")
    parentNode = lastNode.reachNode
    nexts = parentNode.s + 1
    reachNode = StrPosition(nexts,0)
    node = StrReachablePosition(parentNode,reachNode)
    
    shape = environment.shape
    node.parentNode = parentNode
    print("nexts,s of env",nexts , shape[1])
    if nexts < shape[1]:
        if parentNode.l + 1 <= endIndexOfL:
            if environment[parentNode.l + 1][nexts] < 0.1:
                reachNode.l =parentNode.l +1
                node.reachNode = reachNode
                node.g = getGCost(lastNode,reachNode)
                node.h = getHCost(reachNode,stopPosition)
                node.f = getFCost(node)
                openList = updateNode(node,openList,closeMap)

       
        if environment[parentNode.l][nexts] < 0.1:
            reachNode.l =parentNode.l
            node.reachNode = reachNode
            node.g = getGCost(lastNode,reachNode)
            node.h = getHCost(reachNode,stopPosition)
            node.f = getFCost(node)
            openList = updateNode(node,openList,closeMap)
            
        if parentNode.l -1 >= startIndexOfL:
            if environment[parentNode.l -1 ][nexts] < 0.1:
                reachNode.l =parentNode.l -1
                node.reachNode = reachNode
                node.g = getGCost(lastNode,reachNode)
                node.h = getHCost(reachNode,stopPosition)
                node.f = getFCost(node)
                openList = updateNode(node,openList,closeMap)
    return openList
    
def freshCloseList(node,openList,closeList,closeMap):
    print("freshCloseList-------------")
    ikey = node.reachNode.l * numberOfS + node.reachNode.s
    closeList.put(node)
    closeMap[ikey] =node.parentNode.l * numberOfS + node.parentNode.s
    return delMap(node,openList),closeList,closeMap    
     
 
def getMinFFromOpenList(openList):
    print("getMinFFromOpenList-------------")
    #find min node
    start = StrPosition(0,0)
    end = StrPosition(0,0)
    noden = StrReachablePosition(start,end)
    if  len(openList) == 0:
        return noden,False
    minf =999999 
    for nodet in openList.items():
        if minf >nodet[1].f:
            minf = nodet[1].f
            noden = nodet[1]
            
    print("min",minf,noden.reachNode.s,noden.reachNode.l)       
    return noden,True
 


def AStart(environment,startPosition,stopPosition,startIndexOfL,endIndexOfL):
    print("AStart-------------")

    #put startnode to queue
    node = StrReachablePosition(startPosition,startPosition)
    node.g = 0
    node.h = getGCost(node,startPosition)
    node.f = getFCost(node)
    openList = dict()
    openList = refreshMap(node,openList)
    print("dict len",len(openList))
    print("reachPosition",node.reachNode.s,node.reachNode.l)
    time.sleep(1)
    closeList = queue.Queue()
    closeMap = dict()
   #search 
    while node.reachNode.s != stopPosition.s or  
        node.reachNode.l != stopPosition.l and len(openList)!=0:
        # update reachabe node 
        #print("node.reachNode.s,stopPosition.s,node.reachNode.l,stopPosition.l",node.reachNode.s,stopPosition.s,node.reachNode.l,stopPosition.l)
        openList = reachPoints(openList,environment,node,stopPosition,closeMap,startIndexOfL,endIndexOfL)
        #time.sleep(1)
        ##-------------
        for nodet  in openList.items():
            print("openlist",nodet[0],nodet[1].reachNode.s,nodet[1].reachNode.l,nodet[1].f)
        ##----------------
        node,flag = getMinFFromOpenList(openList)
        if flag == False:
            print("openlist null")
            return closeList,closeMap,False
        else:
           openList,closeList,closeMap = freshCloseList(node,openList,closeList,closeMap)
          ##-------------
        #print("closemaplen",len(closeMap))
        #for nodet2  in closeMap.items():
        #    print("closemap",nodet2[0],nodet2[1])
        ##----------------
             
        if node.reachNode.s == stopPosition.s and node.reachNode.l == stopPosition.l:
            return closeList,closeMap,True
    print("openlist null",len(openList))
    return closeList, closeMap,False     
   
def showTrajack(closeMap,startPosition,stopPosition):
    startkey =  startPosition.l * numberOfS + startPosition.s
    endkey = stopPosition.l * numberOfS + stopPosition.s
    
    ikey = closeMap[endkey]
    slist = list()
    llist = list()

    slist.append(stopPosition.s)
    llist.append(stopPosition.l)
    while ikey != startkey:
        s =  np.int(ikey % numberOfS)
        l =  np.int((ikey - s) / numberOfS)
        ikey = closeMap[ikey]
        slist.append(s)
        llist.append(l)
    
    slist.append(startPosition.s)
    llist.append(startPosition.l)
    plt.plot(slist,llist,'*')
    
    
    
    
def showCloseList(closeList):
    while not closeList.empty():
        node = closeList.get()
        plt.plot(node.reachNode.s,node.reachNode.l,'r*')

    
    

if __name__ == '__main__':

    # vehicle to road region  of l 
    startIndexOfL = np.int(vehicleHarfWidth / detaWidth)
    endIndexOfL = numberOfL - np.int(vehicleHarfWidth / detaWidth)
     
    environment = np.zeros((numberOfL,numberOfS))
    createObstacle(environment,obstacle,startIndexOfL,endIndexOfL)
    environmentShow(environment,startIndexOfL,endIndexOfL)
    
    

    #simple end a*--------------------------------------------
    '''startPosition = StrPosition(0,9)
    stopPosition = StrPosition(8,7)
    closeList,closeMap,Flag = AStart(environment,startPosition,stopPosition,startIndexOfL,endIndexOfL)
    
    
    if Flag == True:
        showTrajack(closeMap,startPosition,stopPosition)
    else:
        print("no route-----------------")
        showCloseList(closeList)
    '''
    
    #multi end a*
    
    stopPosition = createStopPosition(numberOfS-1,startIndexOfL,endIndexOfL)  
    startPosition = StrPosition(0,9)

    shapeOfEnd = stopPosition.shape
    for i  in  range(0,shapeOfEnd[0]):
        stop = StrPosition(stopPosition[i][0],stopPosition[i][1])
        closeList,closeMap,Flag = AStart(environment,startPosition,stop,startIndexOfL,endIndexOfL)
        if Flag == True:
            showTrajack(closeMap,startPosition,stop)
        else:
            print("no route-----------------")
        #showCloseList(closeList)