modularization/src/python/cameradecision_signlight_speedcontrol/cameradecision.py

import proto.cameraobjectarray_pb2 as cameraobjectarray_pb2
import proto.decitionspeedlimit_pb2 as decitionspeedlimit_pb2
import proto.lightarray_pb2 as lightarray_pb2
import proto.objectarray_pb2 as objectarray_pb2
import proto.chassis_pb2 as chassis_pb2 

import math
from typing import List  

from datetime import datetime, timedelta  
import time

from xdg.Config import setIconSize


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 m/s²
        self.mmaxwheel = 430  # 最大方向盘角度
        self.mdefaultacc = 1.0  # 加速时的默认加速度
        self.mspeed = 10.0  # 目标速度 10 km/h
        
        self.cspeed = self.mspeed
        self.mstopdistoobs = 6.0  # 距离障碍物6米时停车
        self.mstopdisacc = -1.0  # 障碍物检测时的减速度
        self.mvehwidth = 2.3  # 车辆宽度

        self.speed5_nodetect = 0
        self.is_stop = False

        pass

    def CalcDecision(self, x_chassis, xobjarray_sign, xobjarray_light, xobjarray_lidar):
        acc = 0.0  # 加速度，m/s² (>0 加速, <0 制动)
        wheel = 0.0  # 方向盘转角 (>0 左转, <0 右转, 范围: -430 ~ 430)
        speed = 0.0  # 车速
        leftLamp = False  # 左转向灯
        rightLamp = False  # 右转向灯

        # 获取当前车辆速度
        veh_speed = x_chassis.vel

        print("veh speed: ",veh_speed)

        # 控制加速度，使车速维持在目标速度 mspeed 附近

        if not self.is_stop:
            if veh_speed < self.cspeed:
                acc = min(0.5, self.cspeed - veh_speed)  # 根据差值调整加速度，最大为0.5 m/s²
            elif veh_speed > self.cspeed:
                acc = max(-0.5, self.cspeed - veh_speed)  # 根据差值调整减速度，最小为-0.5 m/s²


        ######################决策逻辑代码########################
        if self.speed5_nodetect < 20:
            self.speed5_nodetect += 1
        # 基于标志物的决策
        for pobj in xobjarray_sign.obj:
            print("sign type: ",pobj.type)
            if pobj.type == "Left":  # 左转标志
                wheel = min(self.mmaxwheel, 30)  # 控制转角
                leftLamp = True  # 打开右转向灯
                
            if pobj.type == "Right":  # 右转标志
                wheel = max(-self.mmaxwheel, -30)  # 控制转角
                rightLamp = True  # 打开左转向灯

            if pobj.type == "Speed5":  # 限速5 km/h标志
                self.cspeed = 5  # 将目标速度调整为 5 km/h
                self.speed5_nodetect = 0
                       
        if self.speed5_nodetect > 10:
            self.cspeed = self.mspeed 

            
        # 基于交通信号灯的决策
        for light in xobjarray_light.light:
            if light.type == 1:  # 绿灯，加速通过
                self.is_stop = False
                acc = 0.5  # 加速
            elif light.type == 2:  # 红灯，停止
                self.is_stop = True
                acc = -1.0  # 大幅减速
        ############################################################

        # 创建并返回决策对象
        xdecisiion = decitionspeedlimit_pb2.decitionspeedlimit()
        xdecisiion.wheelAngle = wheel
        xdecisiion.accelerator = acc
        xdecisiion.brake = 0
        xdecisiion.speed = speed
        xdecisiion.leftLamp = leftLamp
        xdecisiion.rightLamp = rightLamp


        print("acc: ",acc)

        # 制动决策
        if acc < 0:
            xdecisiion.brake = abs(acc)  # 将负加速度转换为制动值
            xdecisiion.torque = 0  # 加速度为负时，无需扭矩输出
        else:
            xdecisiion.brake = 0  # 没有制动
            fVehWeight = 1800  # 车辆重量，单位：kg
            fRollForce = 50  # 滚动阻力
            fRatio = 2.5  # 扭矩比例因子
            fNeed = fRollForce + fVehWeight * acc  # 计算所需的动力
            xdecisiion.torque = 100   # 计算输出扭矩
            if acc == 0:
                xdecisiion.torque = 0
            
        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