|
@@ -0,0 +1,161 @@
|
|
|
+
|
|
|
+
|
|
|
+import proto.decition_pb2 as decition_pb2
|
|
|
+
|
|
|
+import proto.cameraobjectarray_pb2 as cameraobjectarray_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 CameraDecision:
|
|
|
+ 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(xobjarray : cameraobjectarray_pb2.cameraobjectarray):
|
|
|
+
|
|
|
+ nobjsize = len(xobjarray.obj)
|
|
|
+ for pobj in xobjarray.obj:
|
|
|
+ print(f"type: {pobj.type} x : {pobj.x} y: {pobj.y}"); #获取摄像头感知结果的方法,其它数据可以查看proto文件用pobj.{名称}获取
|
|
|
+
|
|
|
+ acc = 0.0 #加速度数值,m/s2
|
|
|
+ wheel = 0.0 #方向盘转角,>0 左转 <0 右转 -430----430
|
|
|
+ speed = 0.0 #车速
|
|
|
+ leftLamp = False
|
|
|
+ rightLamp = False
|
|
|
+
|
|
|
+ ### 在此编写代码给出决策数值
|
|
|
+ ### 在此编写代码给出决策数值
|
|
|
+ ### 在此编写代码给出决策数值
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ xdecisiion = decition_pb2.decition()
|
|
|
+ xdecisiion.wheelAngle = wheel
|
|
|
+ xdecisiion.accelerator = acc
|
|
|
+ xdecisiion.brake = 0
|
|
|
+ xdecisiion.speed = speed
|
|
|
+ xdecisiion.leftLamp = leftLamp
|
|
|
+ xdecisiion.rightLamp = rightLamp
|
|
|
+ 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 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
|