add js_guide decition logic

src/decition/common/common/car_status.h

@@ -243,6 +243,9 @@ namespace iv {
         double guide_amilat = 0.0;
         int guide_lastcarState = 5;//进入云平台人工接管前的状态 0:待命停车 1:任务中 2:返程 3:返库 4:停工停车
         //====================================================================================
+
+        int mbNoPathCnt=0;
+        bool mbNoPath=false;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_jsguide/decition/brain.h

@@ -182,7 +182,7 @@ namespace iv {
             void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
             void adjuseGpsLidarPosition();
             void UpdateGRPCGroupMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-
+            void SideMove(iv::GPS_INS &x);
             iv::group::groupinfo mgroupgrpcInfo;
             qint64 mnGroupgrpcUpdateTime = 0;
             QMutex mMutexGroupgrpc;

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -199,6 +199,10 @@ static int front_car_id=-1;
 static int front_car_vector_id=-1;
 static bool final_brake_lock=false,brake_mode=false;
 
+//20220318JS,终点平滑停车
+bool gfinal_brake=false;
+double gdistance_to_end=1000;
+
 //日常展示
 
 #include <QDateTime>
@@ -1090,17 +1094,20 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         }
                         if(final_brake==true){
                                 if((realspeed>3)&&(final_brake_lock==false)){
-                                            minDecelerate=-0.7;
+                                            //minDecelerate=-0.7;
                                 }else{
                                             dSpeed=min(dSpeed, 3.0);
                                             final_brake_lock=true;
                                             brake_mode=true;
-                                            if(distance_to_end<0.8){
+                                            //if(distance_to_end<0.8){
+                                            if(distance_to_end<0.4){
                                                             minDecelerate=-0.7;
                                             }
 
                                 }
                         }
+                        gfinal_brake=final_brake;
+                        gdistance_to_end=distance_to_end;
                 }
     }
 
@@ -1523,9 +1530,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 #ifdef AVOID_NEW
     if((vehState==avoiding)||(vehState==preAvoid))
     {
-        dSpeed = min(8.0,dSpeed);
+        //dSpeed = min(8.0,dSpeed);
+        dSpeed = min(4.0,dSpeed);
     }else if((vehState==backOri)||(avoidXNew!=0)){
-        dSpeed = min(12.0,dSpeed);
+        //dSpeed = min(12.0,dSpeed);
+        dSpeed = min(5.0,dSpeed);
     }
 #else
     if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
@@ -1543,9 +1552,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //        }
     }
     //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
-    if(brake_mode==true){
-        dSpeed=min(dSpeed, 3.0);
-    }
+//    if(brake_mode==true){
+//        dSpeed=min(dSpeed, 3.0);
+//    }
 
     if(front_car_id>0){
         static bool brake_state=false;
@@ -1900,7 +1909,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         //      if (obsDistance <20 && obsDistance >0)
         if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
         {
-            dSpeed = min(6.0,dSpeed);
+//            dSpeed = min(6.0,dSpeed);
+            dSpeed = min(3.0,dSpeed);
             avoidTimes++;
             //          if (avoidTimes>=6)
             if (avoidTimes>=ServiceCarStatus.aocfTimes)
@@ -2294,6 +2304,46 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 dSpeed=0.0;
             }
         }
+        else
+        {
+            dSpeed=0.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((dSpeed>3.5)&&(realspeed>3.5)&&(gfinal_brake==true) &&(final_brake_lock==false))
+    {
+        dSpeed=realspeed-0.5;
+    }
+
+    if(brake_mode==true)
+    {
+       dSpeed=min(3.0,dSpeed);
+    }
+
+//均胜加速时速度逐渐增加
+    if(dSpeed-realspeed>1.0)
+    {
+        dSpeed=min(realspeed+0.5,dSpeed);
     }
 
     if(ServiceCarStatus.group_control){