V2R logic add

src/decition/common/common/car_status.h

@@ -169,12 +169,12 @@ namespace iv {
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90
         int iTrafficeLightID = 0;       //红绿灯ID
-        int iTrafficeLightColor = 2;    //0x0: 红色
-        //0x1: 黄色
-        //0x2: 绿色
-        int iTrafficeLightTime = 0;     //红绿灯剩余时间
-        double iTrafficeLightLat = 0;
-        double iTrafficeLightLon = 0;
+//        int iTrafficeLightColor = 2;    //0x0: 红色
+//        //0x1: 黄色
+//        //0x2: 绿色
+//        int iTrafficeLightTime = 0;     //红绿灯剩余时间
+//        double iTrafficeLightLat = 0;
+//        double iTrafficeLightLon = 0;
 
         bool daocheMode=false;
 
@@ -229,6 +229,34 @@ namespace iv {
         double distoParkPoint=200;
         std::vector<iv::GPS_INS> xlcParkPoint;//存放停车点经纬的vetor
          //巡逻车增加停车点位.end
+
+        //对于shenlan 1+x车辆V2X信息，cxw,20230223
+        int rsu_traffic_type=0;//路况信息
+       int rsu_warning_type=0;//碰撞预警信息
+        int rsu_radiation_distance=0;//事件辐射范围
+        int rsu_trafficelimit_spd=200;  //200代表没有限速
+        int rsu_warnlimit_spd=200;  //200代表没有限速
+        double rsu_gps_lat=0.0;
+        double rsu_gps_lng=0.0;//中心点
+        int iTrafficeLightColor = 4;    //0x0: 红色
+        //0x1: 黄色
+        //0x2: 绿色
+        int vehicle_state_1x=0; //0正常行驶，1减速，2 停车
+        float target_spd_1x=0;//减速时的目标速度
+        float ui_limit_spd=200;
+
+//        float rsu_limit_spd=200;  //200代表没有限速
+        //int iTrafficeLightColor = 2;    //0x0: 红色
+        //0x1: 黄色
+        //0x2: 绿色
+
+        int iTrafficeLightTime = 200;     //红绿灯剩余时间
+        double iTrafficeLightLat = 0;
+        double iTrafficeLightLon = 0;
+        QTime milightCheckTimer;
+        QTime mRSUupdateTimer;
+        QTime mRSUWarnUpdateTimer;
+        QTime mRSUTrafficUpdateTimer;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -41,6 +41,12 @@ namespace decition {
             gbrain->UpdateSate();
         }
 
+        void ListenV2r(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateV2r(strdata,nSize);
+            gbrain->UpdateSate();
+        }
+
         void ListenHMI(const char * strdata,const unsigned int nSize ,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             gbrain->UpdateHMI(strdata,nSize);
@@ -105,6 +111,8 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpamapreq        = iv::modulecomm::RegisterSend("mapreq",1000,1);
 
+    mpv2r            = iv::modulecomm::RegisterRecv("v2r_send",iv::decition::ListenV2r); //20211009,cxw
+
     ModuleFun funchassis =std::bind(&iv::decition::BrainDecition::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpaChassis = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);
 
@@ -1104,6 +1112,70 @@ void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatas
 
 }
 
+//shenlan2023022320211009,cxw,解析V2R信息，begin
+void iv::decition::BrainDecition::UpdateV2r(const char *pdata, const int ndatasize)
+{
+    iv::v2r::v2r_send group_message;
+    if(false == group_message.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"ListenV2Rmessage Parse fail."<<std::endl;
+        return;
+    }
+    ServiceCarStatus.mRSUupdateTimer.start();
+
+    //由于变量类型改成optional了所以要先判断有没有此变量再赋值,cxw
+    if(group_message.has_mbpause())
+    {
+       ServiceCarStatus.mbRunPause = group_message.mbpause();
+
+//      std::cout<<"enter  for butn %d  "<<ServiceCarStatus.mbRunPause <<std::endl;
+    }
+    if(group_message.has_speedlimit())
+    {
+         ServiceCarStatus.ui_limit_spd = group_message.speedlimit();
+    }
+
+    if(group_message.has_radiobroadcastgpslat())
+    {
+        ServiceCarStatus.rsu_gps_lat = group_message.radiobroadcastgpslat();
+    }
+    if(group_message.has_radiobroadcastgpslon())
+    {
+        ServiceCarStatus.rsu_gps_lng= group_message.radiobroadcastgpslon();
+    }
+    if(group_message.has_radiolighttype()) //1:绿灯 2：红灯 3：黄灯
+    {
+        ServiceCarStatus.iTrafficeLightColor = group_message.radiolighttype();
+    }
+    if(group_message.has_radiolightremain())
+    {
+        ServiceCarStatus.iTrafficeLightTime = group_message.radiolightremain();
+        ServiceCarStatus.milightCheckTimer.start();
+    }
+    if(group_message.has_radiobroadcastrange())
+    {
+        ServiceCarStatus.rsu_radiation_distance = group_message.radiobroadcastrange();
+    }
+    if(group_message.has_radiobroadcasttraffictype())
+    {
+        ServiceCarStatus.rsu_traffic_type = group_message.radiobroadcasttraffictype();
+        ServiceCarStatus.mRSUTrafficUpdateTimer.start();
+    }
+    if(group_message.has_radiobroadcastspeedlimit())
+    {
+        ServiceCarStatus.rsu_trafficelimit_spd = group_message.radiobroadcastspeedlimit();
+    }
+    if(group_message.has_radiowarningtype())
+    {
+        ServiceCarStatus.rsu_warning_type = group_message.radiowarningtype();
+        ServiceCarStatus.mRSUWarnUpdateTimer.start();
+    }
+    if(group_message.has_radiowarningspeedlimit())
+    {
+        ServiceCarStatus.rsu_warnlimit_spd = group_message.radiowarningspeedlimit();
+    }
+}
+
 void iv::decition::BrainDecition::UpdateSate(){
     decitionGps00->isFirstRun=true;
 }

src/decition/decition_brain_sf_changan_shenlan/decition/brain.h

@@ -45,6 +45,7 @@
 #include "ivlog.h"
 #include "formation_map.pb.h"
 #include "vbox.pb.h"
+#include "v2r.pb.h"
 #include "carstate.pb.h"
 #include "groupmsg.pb.h"
 
@@ -131,6 +132,7 @@ namespace iv {
             void * mpaObsTraceLeft;
             void * mpaObsTraceRight;
             void * mpaCarstate;
+             void * mpv2r;//20211009,cxw
 
 
             void ShareDecition(iv::decition::Decition decition);
@@ -159,6 +161,7 @@ namespace iv {
             void UpdateVbox(const char * pdata,const int ndatasize);
             void GetFusion(const char* pdata, const int ndatasize);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
+            void UpdateV2r(const char *pdata, const int ndatasize);
 
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -2132,12 +2132,124 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     std::cout<<"juecesudu0="<<dSpeed<<std::endl;
 
+    //----------------------------------------shenlan 采集车车路协同,add---------------------------------------------
+    if(ServiceCarStatus.mRSUupdateTimer.elapsed()>10*1000)
+     {
+        ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_warning_type=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+     if(ServiceCarStatus.mRSUTrafficUpdateTimer.elapsed()>10*1000)
+     {
+        ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_trafficelimit_spd=200;
+ //       ServiceCarStatus.rsu_warning_type=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+     if(ServiceCarStatus.mRSUWarnUpdateTimer.elapsed()>10*1000)
+     {
+ //       ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_warning_type=200;
+        ServiceCarStatus.rsu_warnlimit_spd=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+    unsigned char traffic_type=ServiceCarStatus.rsu_traffic_type;//路况信息
+    unsigned char warning_type=ServiceCarStatus.rsu_warning_type;//预警信息，RSU获得
+    float distance_to_center=0;
+    GPS_INS traffic_center_gps;
+    traffic_center_gps.gps_lat=ServiceCarStatus.rsu_gps_lat;
+    traffic_center_gps.gps_lng=ServiceCarStatus.rsu_gps_lng;
+    float radiation_distance=ServiceCarStatus.rsu_radiation_distance;//事件辐射范围，从RSU获得
+    float trafficlimit_spd=ServiceCarStatus.rsu_trafficelimit_spd; //从RSU获得的限速值
+    float warnlimit_spd=ServiceCarStatus.rsu_warnlimit_spd; //从RSU获得的限速值
+    //以上变量信息都需要存储到log文件中
+   distance_to_center=GetDistance(now_gps_ins,traffic_center_gps);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
+if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_type==0x04)
+        ||(warning_type==0x01)||(warning_type==0x02))
+{
+   if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
+    {
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10))
+       {
+          dSpeed = min(1.0,realspeed-0.2);
+          ServiceCarStatus.vehicle_state_1x = 1;
+          ServiceCarStatus.target_spd_1x = 1.0;
+       }
+       else if(distance_to_center<radiation_distance)
+       {
+           dSpeed=0.0;
+           minDecelerate=-2.0;
+           ServiceCarStatus.vehicle_state_1x = 2;
+           ServiceCarStatus.target_spd_1x=0.0;
+       }
+       else
+       {}
+    }
+   else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
+    {
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2))
+       {
+          dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
+           ServiceCarStatus.vehicle_state_1x = 1;
+           ServiceCarStatus.target_spd_1x = trafficlimit_spd;
+       }
+       else if(distance_to_center<radiation_distance)
+       {
+          dSpeed=min((double)trafficlimit_spd,realspeed);
+           ServiceCarStatus.vehicle_state_1x = 1;
+           ServiceCarStatus.target_spd_1x = trafficlimit_spd;
+       }
+       else
+       {}
+    }
+    else
+    {}
+//碰撞预警，1减速，2 停车
+    if(warning_type==0x01)
+    {
+        dSpeed=warnlimit_spd;//此处要判断限速值是不是在有效值范围以内
+        ServiceCarStatus.vehicle_state_1x = 1;
+        ServiceCarStatus.target_spd_1x = warnlimit_spd;
+    }
+    else if(warning_type==0x02)
+    {
+        dSpeed=0.0;
+         ServiceCarStatus.vehicle_state_1x = 2;
+         ServiceCarStatus.target_spd_1x = 0;
+    }
+    else
+    {}
+}
+else
+{
+    ServiceCarStatus.vehicle_state_1x = 0;
+    ServiceCarStatus.target_spd_1x = dSpeed;
+}
+
+    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
+
+    //ServiceCarStatus.milightCheckTimer.elapsed();
+    if(ServiceCarStatus.milightCheckTimer.elapsed()>5*1000)
+    {
+       ServiceCarStatus.iTrafficeLightColor=1;//lvdeng
+       ServiceCarStatus.iTrafficeLightTime=200;
+    }
+
     //-------------------------------traffic light----------------------------------------------------------------------------------------
 
     if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
         traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
         //    traffic_light_pathpoint=130;
-        float traffic_speed = ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,gpsMapLine, traffic_light_pathpoint,PathPoint,secSpeed);
+        //float traffic_speed = ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,gpsMapLine, traffic_light_pathpoint,PathPoint,secSpeed);
+        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
+                                                             traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
+
         dSpeed = min((double)traffic_speed,dSpeed);
         if(traffic_speed==0)
         {
@@ -2441,6 +2553,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 void iv::decition::DecideGps00::initMethods(){
 
     pid_Controller= new PIDController();
+
     ge3_Adapter = new Ge3Adapter();
     qingyuan_Adapter = new QingYuanAdapter();
     vv7_Adapter = new VV7Adapter();
@@ -3901,6 +4014,104 @@ float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_col
     return  traffic_speed;
 }
 
+float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                                           int pathpoint,float secSpeed,float dSpeed){
+    float traffic_speed=200;
+    float traffic_dis=0;
+    float passTime;
+    float passSpeed;
+    bool passEnable=false;
+
+    if(abs(secSpeed)<0.1){
+        secSpeed=0;
+    }
+
+
+
+    if(pathpoint <= traffic_light_pathpoint){
+        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }else{
+        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+        for(int i=0;i<traffic_light_pathpoint;i++){
+            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    //    if(traffic_light_color != 0)
+    //    {
+    //        int a = 3;
+    //    }
+
+
+//    if(traffic_light_color==0 && traffic_dis<10){
+//        traffic_speed=0;
+//    }
+
+    if(traffic_light_color==2 && traffic_dis<10){  //cxw,1xhonglvdeng gaibian
+        traffic_speed=0;
+    }
+    //    else   //20200108
+    //    {
+    //        traffic_speed=10;
+    //    }
+    return  traffic_speed;
+
+    passSpeed = min((float)(dSpeed/3.6),secSpeed);
+    passTime = traffic_dis/(dSpeed/3.6);
+
+
+
+//1+x V2R    //1:绿灯 2：红灯 3：黄灯
+    //最开始0x0: 红色 0x1: 黄色 0x2: 绿色
+
+    switch(traffic_light_color){
+    case 2://case 0: //for 1+x修改
+        if(passTime>traffic_light_time+1 && traffic_dis>10){
+            passEnable=true;
+        }else{
+            passEnable=false;
+        }
+        break;
+   case 3:// case 1:
+        if(passTime<traffic_light_time-1 && traffic_dis<10){
+            passEnable=true;
+        }else{
+            passEnable = false;
+        }
+        break;
+    case 1://case 2:
+        if(passTime<traffic_light_time){
+            passEnable= true;
+        }else{
+            passEnable=false;
+        }
+        break;
+
+
+    default:
+        break;
+    }
+
+    if(!passEnable){
+        if(traffic_dis<5){
+            traffic_speed=0;
+        }else if(traffic_dis<10){
+            traffic_speed=5;
+        }else if(traffic_dis<20){
+            traffic_speed=15;
+        }else if(traffic_dis<30){
+            traffic_speed=25;
+        }else if(traffic_dis<50){
+            traffic_speed=30;
+        }
+    }
+    return traffic_speed;
+
+}
 void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs,
                                                          const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
 {

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -243,6 +243,8 @@ public:
 
     bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
+    float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                                               int pathpoint,float secSpeed,float dSpeed);
 
 
     void transferGpsMode2(const std::vector<GPSData> gpsMapLine);

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -33,6 +33,7 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/fusionobjectarray.pb.cc \
     ../../include/msgtype/fusionobject.pb.cc \
     ../../include/msgtype/carstate.pb.cc \
+    ../../include/msgtype/v2r.pb.cc \
     ../../include/msgtype/groupmsg.pb.cc
 
 
@@ -97,6 +98,9 @@ HEADERS += \
     ../common/perception_sf/sensor_radar.h \
     ../common/common/sysparam_type.h \
     ../../include/msgtype/carstate.pb.h \
+
+    ../../include/msgtype/v2r.pb.h \
+
     ../../include/msgtype/groupmsg.pb.h