add JS runlegs statelogic

src/decition/common/common/car_status.h

@@ -229,6 +229,14 @@ namespace iv {
         double distoParkPoint=200;
         std::vector<iv::GPS_INS> xlcParkPoint;//存放停车点经纬的vetor
          //巡逻车增加停车点位.end
+
+       bool mbReceiveTask=false; //js 的车辆增加状态机
+       bool mbFulfilTask=false;
+       bool mbCloseToPath =false;
+       bool mbAwayFromPath=false;
+       int  mnReceiveTaskCnt=0;
+       bool mbNoPath=false;
+       int mbNoPathCnt=0;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_jsrunlegs/decition/brain.cpp

@@ -480,8 +480,25 @@ void iv::decition::BrainDecition::run() {
                 std::cout<<"iv::map::mapreq serialize error."<<std::endl;
             }
             delete str;
-        }
 
+            ServiceCarStatus.mbReceiveTask=false;
+            ServiceCarStatus.mbFulfilTask = false;
+            ServiceCarStatus.mbCloseToPath = false;
+            ServiceCarStatus.mbAwayFromPath = false;
+            ServiceCarStatus.mnReceiveTaskCnt=0;
+        }
+        else
+        {
+//          ServiceCarStatus.mnReceiveTaskCnt++;
+//          if(ServiceCarStatus.mnReceiveTaskCnt<=100) //维持2秒中
+//          {
+//            ServiceCarStatus.mbReceiveTask =true;
+//          }
+//          else
+//          {
+//              ServiceCarStatus.mbReceiveTask=false;
+//          }
+        }
 
         iv::GPSData gps_data_;			//gps 惯导数据
 
@@ -502,7 +519,7 @@ void iv::decition::BrainDecition::run() {
         std::cout<<"mbRunPause==="<<ServiceCarStatus.mbRunPause<<endl;
         std::cout<<"mapsize==="<<navigation_data.size()<<endl;
         std::cout<<"boche==="<<ServiceCarStatus.mnBocheComplete<<endl;
-         std::cout<<"*********************************************"<<endl;
+        std::cout<<"*********************************************"<<endl;
         if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
         {
             ServiceCarStatus.mbBrainCtring = false;
@@ -524,6 +541,11 @@ void iv::decition::BrainDecition::run() {
             xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
             xbs.set_mfobs(ServiceCarStatus.mObs);
             xbs.set_decision_period(decision_period);
+            xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
+            xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
+            xbs.set_mbfulfiltask(ServiceCarStatus.mbFulfilTask);
+            xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
+            xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
             ShareBrainstate(xbs);    //apollo_fu 20200413 qudiaozhushi
             //            iv::decition::VehicleStateBasic vsb;
             //            vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
@@ -861,6 +883,11 @@ void iv::decition::BrainDecition::run() {
             xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
             xbs.set_mfobs(ServiceCarStatus.mObs);
             xbs.set_decision_period(decision_period);
+            xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
+            xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
+            xbs.set_mbfulfiltask(ServiceCarStatus.mbFulfilTask);
+            xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
+            xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
             ShareBrainstate(xbs);
 
 

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -357,9 +357,25 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         obstacle_avoid_flag=0;
         avoidXNew=0;
 
+        ServiceCarStatus.mbReceiveTask =true;  //JS状态机
+        ServiceCarStatus.mnReceiveTaskCnt=0;
+
+        ServiceCarStatus.mbFulfilTask = false;
+        ServiceCarStatus.mbCloseToPath = false;
+        ServiceCarStatus.mbAwayFromPath = false;
         givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",
                         0,0);
     }
+
+     if(ServiceCarStatus.mnReceiveTaskCnt<=100)
+     {
+       ServiceCarStatus.mnReceiveTaskCnt++;
+     }
+     else
+     {
+         ServiceCarStatus.mbReceiveTask=false;
+     }
+
     ServiceCarStatus.mvehState=vehState;
     ServiceCarStatus.mavoidX=avoidXNew;
 
@@ -1028,12 +1044,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (PathPoint<0)
     {
-
+        ServiceCarStatus.mbCloseToPath = false;
+        ServiceCarStatus.mbAwayFromPath = true;
         minDecelerate=-1.0;
         phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
         return gps_decition;
 
     }
+    else
+    {
+        ServiceCarStatus.mbCloseToPath = true;  //JS状态机
+        ServiceCarStatus.mbAwayFromPath = false;
+    }
 
     DecideGps00::lastGpsIndex = PathPoint;
 
@@ -1103,7 +1125,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                             final_brake_lock=true;
                                             brake_mode=true;
                                             if(distance_to_end<0.8){
-                                                            minDecelerate=-0.7;
+                                                minDecelerate=-0.7;
+                                                ServiceCarStatus.mbFulfilTask = true;
                                             }
 
                                 }
@@ -2285,24 +2308,52 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if(obsDistance>0 && obsDistance<8){
                 dSpeed=0;
             }
-    }else if(obsDistance>0 && obsDistance<15){   //均胜跑腿车距离可以适当减小
+    }
+    else if(obsDistance>0 && obsDistance<15)
+    {   //均胜跑腿车距离可以适当减小
 
         if(ServiceCarStatus.msysparam.mvehtype=="haomo")
         {
-            if(obsDistance>ServiceCarStatus.socfDis && obsDistance<15)
+//            if(obsDistance>ServiceCarStatus.socfDis && obsDistance<15)
+//            {
+//                dSpeed = min(3.0,dSpeed);
+//            }
+//            else
+            if((obsDistance>ServiceCarStatus.socfDis-4) &&obsDistance<ServiceCarStatus.socfDis) //
             {
-                dSpeed = min(3.0,dSpeed);
+//               dSpeed=0;
+               dSpeed = min(3.0,dSpeed);
             }
-            else if(obsDistance<ServiceCarStatus.socfDis)
+            else
             {
-               dSpeed=0;
+                dSpeed=0;
             }
-
-
         }
         else
+        {
             dSpeed=0;
+        }
+    }
+
+    //JS 车辆被obs阻挡无路可走
+    if((obsDistance>0) &&obsDistance<(ServiceCarStatus.socfDis-4))
+    {
+      if(dSpeed==0)
+      {
+          ServiceCarStatus.mbNoPathCnt++;
+      }
+      if(ServiceCarStatus.mbNoPathCnt>100)
+      {
+          ServiceCarStatus.mbNoPath=true;
+          ServiceCarStatus.mbNoPathCnt=ServiceCarStatus.mbNoPathCnt;
+      }
     }
+    else
+    {
+       ServiceCarStatus.mbNoPath=false;
+       ServiceCarStatus.mbNoPathCnt=0;
+    }
+//
 
     if(ServiceCarStatus.group_control){
         dSpeed = ServiceCarStatus.group_comm_speed;

src/include/proto/brainstate.proto

@@ -10,4 +10,9 @@ message brainstate
   optional bool mbBocheEnable = 4;
   optional double decision_period= 5; 
   optional bool mbBrainRunning = 6;
+  optional bool mbReceiveTask=7;
+  optional bool mbFulfilTask=8;
+  optional bool mbCloseToPath =9;
+  optional bool mbAwayFromPath=10;
+  optional bool mbNoPath=11;
 };