modularization/src/python/pythondecisiondemo/decisiondemo.py

import proto.gpsimu_pb2 as gpsimu_pb2
#import gpsimu_pb2
import proto.mapdata_pb2 as mapdata_pb2
import proto.decition_pb2 as decition_pb2

from proto import objectarray_pb2 as objectarray_pb2

import math
from typing import List  

from datetime import datetime, timedelta  
import time  

class Point2D:
    def __init__(self, x, y,hdg):
        self.mx = x
        self.my = y
        self.mhdg = hdg
        while self.mhdg < 0:
            self.mhdg = self.mhdg + 2.0* math.pi
        while self.mhdg >= 2.0*math.pi:
            self.mhdg = self.mhdg - 2.0* math.pi
        

    def __str__(self):  
        return f"Point2D({self.mx}, {self.my})"  

class DecisionDemo:  
    def __init__(self):  
        # 初始化代码...  
        self.mendacc = -0.7  #抵达终点时的舰速度为-0.7
        self.mmaxwheel = 430 #最大方向盘角度
        self.mdefaultacc = 1.0  #加速时的acc
        self.mspeed = 10.0  #运行速度10km/h
        self.mstopdistoobs = 6.0 #在距离障碍物多远停住
        self.mstopdisacc = -1.0  #有障碍物时的舰速度
        self.mvehwidth = 2.3
        pass

    def CalcDecision(self,tracemap : mapdata_pb2.tracemap, msg_gpsimu : gpsimu_pb2.gpsimu,xobjarray : objectarray_pb2.objectarray):
  #      x,y = self.GaussProj(gpsimu_pb2.lon,gpsimu_pb2.lat)
 #       print(" in calc: map size: ",len(tracemap.point))
        nearindex = self.FindNearIndex(tracemap,msg_gpsimu)
 #       print("near index: ",nearindex)

 #       for p in xobjarray.obj :
 #           print(f"x: {p.position.x}  y: {p.position.y} ")

        if nearindex < 0 :
            print(" not found near point, return")
            return
        
        localpoints,distoend = self.CalcLocalPath(nearindex,tracemap,msg_gpsimu)
 #       print(f"disttoend: {distoend}")

        realspeed = 3.6 * math.sqrt(math.pow(msg_gpsimu.vn,2) + math.pow(msg_gpsimu.ve,2))

        acc,wheel,speed = self.CalcCmd(localpoints,realspeed,xobjarray)

        #calc end acc, stop to end point
        endacc = 0.0
        if distoend > 0.1:
            if distoend < math.pow(realspeed/3.6,2)/(2*math.fabs(self.mendacc)) :
                endacc = math.pow(realspeed/3.6,2)/(2*distoend) *(-1.0)
                print("endacc: ",endacc)
        else :
            endacc = self.mendacc
        
 #       print("endacc: ",endacc)
        if endacc<-1e-9:
            acc = endacc

        if acc < -5.0:
            acc = -5.0


        xdecisiion = decition_pb2.decition()
        xdecisiion.wheelAngle = wheel
        xdecisiion.accelerator = acc
        xdecisiion.brake = 0
        xdecisiion.speed = speed
        print("acc = : ",acc)
        if acc < 0:
            xdecisiion.brake = acc
            xdecisiion.torque = 0
        else:
            xdecisiion.brake = 0
            fVehWeight = 1800
 #           fg = 9.8
            fRollForce = 50
            fRatio = 2.5
            fNeed = fRollForce + fVehWeight*acc
            xdecisiion.torque = fNeed/fRatio

        return xdecisiion

        

    


    def CalcCmd(self,localpoints : List[Point2D],realspeed,xobjarray : objectarray_pb2.objectarray):
        desiredspeed = self.mspeed
        defaultacc = self.mdefaultacc
        pd = realspeed * 0.3
        if pd < 4.0 : 
            pd = 4.0
        
        sumdis = 0
        i = 1
        size = len(localpoints)
        ppindex = 0
        
        while i < size:
            sumdis = sumdis + math.sqrt(math.pow(localpoints[i].mx - localpoints[i-1].mx,2) 
                               + math.pow(localpoints[i].my - localpoints[i-1].my,2))
            ppindex = i
            if sumdis >= pd :
                ppindex = i
                break
            i = i+1
        acc = -0.5
        wheel = 0.0
        if ppindex < 3:
            acc = 0.0
            wheel = 0.0
            return acc,wheel,0
        
        if desiredspeed > 0.1:
            if realspeed < desiredspeed :
                if realspeed < desiredspeed*0.9:
                    acc = defaultacc
                else:
                    acc = defaultacc *(desiredspeed - realspeed) * (1/0.1)/desiredspeed
            else:
                if realspeed > desiredspeed*1.1:
                    if realspeed > desiredspeed * 2.0:
                        acc = defaultacc * (-1.0)
                    else:
                        acc = defaultacc *  (desiredspeed - realspeed)/desiredspeed
                else:
                    acc = 0.0
        
        obsdis = 0
        bhaveobs = False

        i = 0
        gridsize = 0.2
        gridcount = int((self.mvehwidth/2.0)/gridsize)*2
        xwid = gridcount * gridsize
        if xwid < self.mvehwidth:
            gridcount = gridcount +2
        while i< size:
            if i > 0:
                obsdis = obsdis + math.sqrt(math.pow(localpoints[i].mx - localpoints[i-1].mx,2)
                                            +math.pow(localpoints[i].my - localpoints[i-1].my,2))

            nobjsize = len(xobjarray.obj)
            j = 0
            bprob = False  #查看有没有可能
            for pobj in xobjarray.obj:
                dis = math.sqrt(math.pow(localpoints[i].mx - pobj.position.x,2)+ math.pow(localpoints[i].my - pobj.position.y,2))
 #               print("dis: ",dis)
 #               print("dimen: ",pobj.dimensions.x)
                if (dis < (self.mvehwidth/2.0 + pobj.dimensions.x)) or (dis < (self.mvehwidth/2.0 + pobj.dimensions.y)):
                    bprob = True
                if (bprob == True):
                    break
            if bprob == False:
                i = i+1
                continue
            j =0
            while j <= gridcount:
                off = (j - gridcount/2)*gridsize
                xpos = localpoints[i].mx + off * math.cos(localpoints[i].mhdg + math.pi/2.0)
                ypos = localpoints[i].my + off * math.sin(localpoints[i].mhdg + math.pi/2.0)
                for pobj in xobjarray.obj:
                    bres = self.is_point_in_rotated_rectangle(xpos,ypos,pobj.position.x,pobj.position.y,pobj.tyaw,
                                                              pobj.dimensions.x,pobj.dimensions.y)
                    if bres == True:
                        bhaveobs = True
                        break
                if bhaveobs == True:
                    break
                j = j+1
            if bhaveobs == True:
                break
            i = i+1

        if bhaveobs == True :
            print("obs dis: ",obsdis)
            vel = realspeed/3.6
            srange = vel*vel/(2*(math.fabs(self.mstopdisacc)/2.0)) + self.mstopdistoobs
            if obsdis <= srange:
                if obsdis < (self.mstopdistoobs + 0.1):
                    stopacc  = -3.0
                    if acc > stopacc:
                        acc = stopacc
                else :
                    stopacc = -1.0*vel * vel/(2.0*(obsdis - self.mstopdistoobs))
                    if acc > stopacc:
                        acc = stopacc
            if obsdis < self.mstopdistoobs * 1.1:
                if acc >= 0:
                    acc = self.mstopdisacc
    
#        print("ppindex : ",ppindex)
        denominator = 2 * localpoints[ppindex].mx *(-1);
        numerator = math.pow(localpoints[ppindex].mx,2) + pow(localpoints[ppindex].my,2)
        fRadius = 1e9
        if math.fabs(denominator)>0:
            fRadius = numerator/denominator
        else:
            fRadius = 1e9
        if fRadius == 0:
            wheel = 0
        kappa = 1.0/fRadius
        wheel_base = 2.9
        wheelratio = 13.6
        wheel = (1.0)*kappa * wheel_base * wheelratio * 180.0/math.pi
        if wheel>self.mmaxwheel:
            wheel = self.mmaxwheel
        if wheel<(self.mmaxwheel * (-1.0)):
            wheel = self.mmaxwheel * (-1.0)

        return acc,wheel,self.mspeed
        

    def FindNearIndex(self,tracemap : mapdata_pb2.tracemap,xgpsimu : gpsimu_pb2.gpsimu) :
        x,y = self.GaussProj(xgpsimu.lon,xgpsimu.lat)
        nearindex = -1
        neardis = 100000
        index = 0
        for p in tracemap.point:
            dis = math.sqrt(math.pow(x - p.gps_x,2) + math.pow(y - p.gps_y,2))
            headingdiff = xgpsimu.heading - p.ins_heading_angle
            while headingdiff < -180:
                headingdiff = headingdiff + 360
            while headingdiff >= 180:
                headingdiff = headingdiff - 360
            if math.fabs(headingdiff) < 80 :
                if dis < neardis :
                    neardis = dis
                    nearindex = index
            index = index + 1
        return nearindex
    
    def CalcLocalPath(self,index,xtracemap : mapdata_pb2.tracemap,xgpsimu : gpsimu_pb2.gpsimu):
        x0,y0 = self.GaussProj(xgpsimu.lon,xgpsimu.lat)
        localpoints = []
        i = index
        npoint = 0
        nmaxpoint = 600
        length = len(xtracemap.point)
        distoend = 0;
        while i< length :
            xraw = xtracemap.point[i].gps_x - x0
            yraw = xtracemap.point[i].gps_y - y0
     #       print(f"i: {i-index} xraw:{xraw} yraw:{yraw} ")
            theta = (90 - xgpsimu.heading) * math.pi /180.0
            thetaraw = theta
            pointhead = (90 - xtracemap.point[i].ins_heading_angle ) * math.pi/180.0;
            theta = theta * (-1)
            theta = theta  + math.pi/2.0
            x = xraw * math.cos(theta) - yraw * math.sin(theta)
            y = xraw * math.sin(theta) + yraw * math.cos(theta)
 #           print(f"i: {i-index} x:{x} y:{y} hdg:{pointhead - thetaraw  + math.pi/2.0}")
            localpoints.append(Point2D(x,y, pointhead - thetaraw  + math.pi/2.0))
            if i>index :
                distoend = distoend + math.sqrt(math.pow(xtracemap.point[i].gps_x - xtracemap.point[i-1].gps_x,2) 
                                                + math.pow(xtracemap.point[i].gps_y - xtracemap.point[i-1].gps_y,2))
            if npoint >= nmaxpoint:
                break
            i = i+1
            npoint = npoint + 1
        while i < length :
            if i>index :
                distoend = distoend + math.sqrt(math.pow(xtracemap.point[i].gps_x - xtracemap.point[i-1].gps_x,2) 
                                                + math.pow(xtracemap.point[i].gps_y - xtracemap.point[i-1].gps_y,2))
            i = i+1
            if distoend > 300:
                break
        return localpoints,distoend
    
    def is_point_in_rotated_rectangle(self,x, y, x1, y1, yaw, l, w):  
        # 将长方形的左下角坐标转换到原点  
        x_rel = x - x1  
        y_rel = y - y1  
  
        # 计算旋转矩阵（逆时针旋转）  
        # | cos(yaw)  -sin(yaw) |  
        # | sin(yaw)   cos(yaw) |  
        cos_yaw = math.cos(yaw)  
        sin_yaw = math.sin(yaw)  
      
        # 应用旋转矩阵到相对坐标  
        x_rotated = x_rel * cos_yaw + y_rel * sin_yaw  
        y_rotated = -x_rel * sin_yaw + y_rel * cos_yaw  
  
        # 判断点是否在旋转后的长方形内  
        # 长方形的边界在旋转后的坐标系中是 [-l/2, l/2] x [-w/2, w/2]  
        if -l/2 <= x_rotated <= l/2 and -w/2 <= y_rotated <= w/2:  
            return True  
        else:  
            return False  
  

    def GaussProj(self,lon,lat):
        iPI = 0.0174532925199433
        ZoneWide = 6
        a = 6378245.0
        f = 1.0 / 298.3
        ProjNo = int(lon / ZoneWide)
        longitude0 = ProjNo * ZoneWide + ZoneWide / 2
        longitude0 = longitude0 * iPI
        latitude0 = 0
        longitude1 = lon * iPI #经度转换为弧度
        latitude1 = lat * iPI #//纬度转换为弧度
        e2 = 2 * f - f * f
        ee = e2 * (1.0 - e2)
        NN = a / math.sqrt(1.0 - e2 * math.sin(latitude1)*math.sin(latitude1))
        T = math.tan(latitude1)*math.tan(latitude1)
        C = ee * math.cos(latitude1)*math.cos(latitude1)
        A = (longitude1 - longitude0)*math.cos(latitude1)
        M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2*e2 / 1024)*math.sin(2 * latitude1)+ (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*math.sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*math.sin(6 * latitude1))
        xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120)
        yval = M + NN * math.tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24 + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720)
        X0 = 1000000 * (ProjNo + 1) + 500000
        Y0 = 0
        xval = xval + X0; yval = yval + Y0;
        X = xval
        Y = yval
        return X,Y
    
    def GaussProjInvCal(self,X,Y):
        iPI = 0.0174532925199433   #3.1415926535898/180.0;
        a = 6378245.0
        f = 1.0 / 298.3 # //54年北京坐标系参数
                #////a=6378140.0; f=1/298.257; //80年西安坐标系参数
        ZoneWide = 6  # ////6度带宽
        ProjNo = int(X / 1000000)  # //查找带号
        longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2
        longitude0 = longitude0 * iPI  # //中央经线
        X0 = ProjNo * 1000000 + 500000  
        Y0 = 0
        xval = X - X0; yval = Y - Y0 #//带内大地坐标
        e2 = 2 * f - f * f 
        e1 = (1.0 - math.sqrt(1 - e2)) / (1.0 + math.sqrt(1 - e2))
        ee = e2 / (1 - e2)
        M = yval
        u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256))
        fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*math.sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*math.sin(4 * u)+ (151 * e1*e1*e1 / 96)*math.sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*math.sin(8 * u)
        C = ee * math.cos(fai)*math.cos(fai)
        T = math.tan(fai)*math.tan(fai)
        NN = a / math.sqrt(1.0 - e2 * math.sin(fai)*math.sin(fai))
        R = a * (1 - e2) / math.sqrt((1 - e2 * math.sin(fai)*math.sin(fai))*(1 - e2 * math.sin(fai)*math.sin(fai))*(1 - e2 * math.sin(fai)*math.sin(fai)))
        D = xval / NN
        #//计算经度(Longitude) 纬度(Latitude)
        longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D*D*D*D*D / 120) / math.cos(fai)
        latitude1 = fai - (NN*math.tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24 + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720)
        #//转换为度 DD
        longitude = longitude1 / iPI
        latitude = latitude1 / iPI
        return longitude,latitude