add v2r logic and share vehicle state

src/decition/common/common/car_status.h

@@ -212,10 +212,15 @@ namespace iv {
         unsigned char rsu_traffic_type=0;//路况信息
         unsigned char rsu_warning_type=0;//碰撞预警信息
         float rsu_radiation_distance=0;//事件辐射范围
-        float rsu_limit_spd=200;  //200代表没有限速
+        float rsu_trafficelimit_spd=200;  //200代表没有限速
+        float rsu_warnlimit_spd=200;  //200代表没有限速
+        double rsu_gps_lat=0.0;
+        double rsu_gps_lng=0.0;//中心点
         int iTrafficeLightColor = 2;    //0x0: 红色
         //0x1: 黄色
         //0x2: 绿色
+        int vehicle_state_1x=0; //0正常行驶，1减速，2 停车
+        float target_spd_1x=0;//减速时的目标速度
         int iTrafficeLightTime = 0;     //红绿灯剩余时间
         double iTrafficeLightLat = 0;
         double iTrafficeLightLon = 0;

src/decition/decition_brain_sf_1x/decition/brain.cpp

@@ -38,6 +38,12 @@ iv::decition::BrainDecition * gbrain;
             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);
@@ -134,6 +140,8 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpvbox = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);
 
+    mpv2r = iv::modulecomm::RegisterRecv("v2r",iv::decition::ListenV2r); //20211009,cxw
+
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
 
     mpfusion = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);//20210929，cxw,buyong zhege
@@ -732,7 +740,7 @@ void iv::decition::BrainDecition::run() {
                 mfVehSpeedLast=ServiceCarStatus.vehSpeed_st;
             }
 
-//            iv::LidarGridPtr templidar= obs_lidar_grid_;
+//            iv::LidarGridPtr templidar= obs_lidar_grid_;//如果单纯用原来的激光检测站该物需要把这个放开，把下面的屏蔽，否则还是没有障碍物，check
             iv::LidarGridPtr  templidar;
             templidar = NULL;
             mMutex_.lock();
@@ -841,6 +849,10 @@ void iv::decition::BrainDecition::run() {
             xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
             xbs.set_mfobs(ServiceCarStatus.mObs);
             xbs.set_decision_period(decision_period);
+            //20211011,cxw,增加1x车辆行驶状态和目标限速，begin
+            xbs.set_vehicle_state(ServiceCarStatus.vehicle_state_1x);
+            xbs.set_vehicle_spd(ServiceCarStatus.target_spd_1x);
+            //20211011,cxw,增加1x车辆行驶状态和目标限速，end
             ShareBrainstate(xbs);
 
 
@@ -1306,7 +1318,12 @@ void iv::decition::BrainDecition::UpdateHMI(const char *pdata, const int ndatasi
         return;
     }
 
-    ServiceCarStatus.mbRunPause = xhmimsg.mbpause();
+    //由于变量类型改成optional了所以要先判断有没有此变量再赋值,cxw
+    if(xhmimsg.has_mbpause())
+    {
+       ServiceCarStatus.mbRunPause = xhmimsg.mbpause();
+    }
+//    ServiceCarStatus.mbRunPause = xhmimsg.mbpause();
 
     if(xhmimsg.mbbochemode()){
          ServiceCarStatus.bocheMode = true;
@@ -1629,6 +1646,54 @@ void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatas
 
 }
 
+
+//20211009,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;
+    }
+    if(group_message.has_radiobroadcastgpslat())
+    {
+        ServiceCarStatus.rsu_gps_lat = group_message.radiobroadcastgpslat();
+    }
+    if(group_message.has_radiobroadcastgpslon())
+    {
+        ServiceCarStatus.rsu_gps_lat = group_message.radiobroadcastgpslon();
+    }
+    if(group_message.has_radiolighttype())
+    {
+        ServiceCarStatus.iTrafficeLightColor = group_message.radiolighttype();
+    }
+    if(group_message.has_radiolightremain())
+    {
+        ServiceCarStatus.iTrafficeLightTime = group_message.radiolightremain();
+    }
+    if(group_message.has_radiobroadcastrange())
+    {
+        ServiceCarStatus.rsu_radiation_distance = group_message.radiobroadcastrange();
+    }
+    if(group_message.has_radiobroadcasttraffictype())
+    {
+        ServiceCarStatus.rsu_traffic_type = group_message.radiobroadcasttraffictype();
+    }
+    if(group_message.has_radiobroadcastspeedlimit())
+    {
+        ServiceCarStatus.rsu_trafficelimit_spd = group_message.radiobroadcastspeedlimit();
+    }
+    if(group_message.has_radiowarningtype())
+    {
+        ServiceCarStatus.rsu_warning_type = group_message.radiowarningtype();
+    }
+    if(group_message.has_radiowarningspeedlimit())
+    {
+        ServiceCarStatus.rsu_warnlimit_spd = group_message.radiowarningspeedlimit();
+    }
+}
+//20211009,cxw,解析V2R信息，end
 void iv::decition::BrainDecition::UpdateSate(){
      decitionGps00->isFirstRun=true;
 }

src/decition/decition_brain_sf_1x/decition/brain.h

@@ -45,6 +45,7 @@
 #include "ivlog.h"
 #include "formation_map.pb.h"
 #include "vbox.pb.h"
+#include "v2r.pb.h" //20211009,cxw
 
 #include "fanyaapi.h"
 
@@ -138,6 +139,7 @@ namespace iv {
             void * mpaPlanTrace;
             void * mpaObsTraceLeft;
             void * mpaObsTraceRight;
+            void * mpv2r;//20211009,cxw
 
 
             void ShareDecition(iv::decition::Decition decition);
@@ -162,6 +164,7 @@ namespace iv {
             void UpdateGroupInfo(const char * pdata,const int ndatasize);
             void UpdateSate();
             void UpdateVbox(const char * pdata,const int ndatasize);
+            void UpdateV2r(const char * pdata,const int ndatasize);//20211009,cxw
             void GetFusion(const char* pdata, const int ndatasize);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
             void UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs);

src/decition/decition_brain_sf_1x/decition/decide_gps_00.cpp

@@ -1989,41 +1989,76 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     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 limit_spd=ServiceCarStatus.rsu_limit_spd; //从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,gps_center);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
-    if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
+   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.5);
+          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)
+       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)limit_spd,realspeed-0.5);
+          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=limit_spd;//此处要判断限速值是不是在有效值范围以内
+        dSpeed=warnlimit_spd;//此处要判断限速值是不是在有效值范围以内
+        ServiceCarStatus.vehicle_state_1x = 1;
+        ServiceCarStatus.target_spd_1x = warnlimit_spd;
     }
-    else if(warning_type==0x01)
+    else if(warning_type==0x02)
     {
         dSpeed=0.0;
+         ServiceCarStatus.vehicle_state_1x = 2;
+         ServiceCarStatus.target_spd_1x = 0;
     }
-//红绿灯信息处理，computeTrafficSpeedLimt由于车子最高5所以要对速度做调整
+    else
+    {}
+}
+else
+{
+    ServiceCarStatus.vehicle_state_1x = 0;
+}
+    //红绿灯信息处理，computeTrafficSpeedLimt由于车子最高5所以要对速度做调整
+
     //-----------------------------------------1+X采集车车路协同，end---------------------------------------------
     //-------------------------------traffic light----------------------------------------------------------------------------------------
 
@@ -2177,7 +2212,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                   <<"OBS_Speed"<< ","<<obsSpeed<< ","
                   <<"Traffic_Type"<< ","<<traffic_type<< ","
                   <<"Warn_Type"<< ","<<warning_type<< ","
-                  <<"Limit_Speed"<< ","<<limit_spd<< ","
+                  <<"Limit_Speed"<< ","<<trafficlimit_spd<< ","
                   <<endl;
            outfile.close();
        }
@@ -2240,13 +2275,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
            else
            {
                float ttc = 0-(obsDistance/obsSpeed);
+               double obsDistance_log=0;
+               if(obsDistance>500)
+                   obsDistance_log=100;
+               else
+                   obsDistance_log=obsDistance;
                QDateTime dt2=QDateTime::currentDateTime();
                outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
                       <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
                       <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
                       <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                       <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
-                      <<"OBS_Distance"<< ","<<obsDistance<< ","
+                      <<"OBS_Distance"<< ","<<obsDistance_log<< ","
                       <<"Min_Decelation"","<<minDecelerate<< ","
                       <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
                       <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
@@ -2255,12 +2295,22 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                       <<"Trace_Point"<< ","<<PathPoint<< ","
                       <<"OBS_Speed"<< ","<<obsSpeed<< ","
                       <<"TTC"<< ","<<ttc<< ","
+                      <<"Traffic_Type"<< ","<<traffic_type<< ","
+                      <<"Warn_Type"<< ","<<warning_type<< ","
+                      <<"TrafficLimit_Speed"<< ","<<trafficlimit_spd<< ","
+                      <<"Radation_Dis"<< ","<<radiation_distance<< ","
+                      <<"Dis_to_center"<< ","<<distance_to_center<< ","
                       <<endl;
                outfile.close();
            }
    }
-       givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f",
-               vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle);
+       givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,"
+                     "obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,"
+                     "wheel_angle: %f","Traffic_Type: %d","Warn_Type: %d","TrafficLimit_Speed: %f",
+                     "radiation_dis: %f","Dis_to_center: %f","WarnLimit_Speed: %f",
+                      vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,
+                     gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,traffic_type,
+                     warning_type,trafficlimit_spd,radiation_distance,distance_to_center,warnlimit_spd);
        }
 
 

src/decition/decition_brain_sf_1x/decition_brain_sf_1x.pro

@@ -32,7 +32,7 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/formation_map.pb.cc \
     ../../include/msgtype/fusionobjectarray.pb.cc \
     ../../include/msgtype/fusionobject.pb.cc \
-
+    ../../include/msgtype/v2r.pb.cc
 
 include($$PWD/../common/common/common.pri)
 include($$PWD/decition/decition.pri)
@@ -93,6 +93,7 @@ HEADERS += \
     ../common/perception_sf/sensor_gps.h \
     ../common/perception_sf/sensor_lidar.h \
     ../common/perception_sf/sensor_radar.h \
-    ../common/common/sysparam_type.h
+    ../common/common/sysparam_type.h\
+    ../../include/msgtype/v2r.pb.h
 
 

src/include/proto/brainstate.proto

@@ -10,4 +10,6 @@ message brainstate
   optional bool mbBocheEnable = 4;
   optional double decision_period= 5; 
   optional bool mbBrainRunning = 6;
+  optional int32 vehicle_state=7;//1+x,车辆状态，0正常行驶，1减速，2 停车
+  optional float vehicle_spd=8;//1+x,车辆减速时的目标速度
 };