添加状态机的一些逻辑

src/decition/common/common/car_status.h

@@ -230,17 +230,19 @@ namespace iv {
         std::vector<iv::GPS_INS> xlcParkPoint;//存放停车点经纬的vetor
          //巡逻车增加停车点位.end
 
-        //=========================apollo_fu 2022=======================
-        int guide_carState = 5;	// 0:待命停车 1:任务中	 2:返程 3:返库 4:停工停车 5:人工接管
+        //=========================apollo_fu 2022=============================================
+        int guide_carState = 5;	// 0:待命停车 1:任务中	 2:返程 3:返库 4:停工停车 5:云平台人工接管
         bool guide_faultstate = 1;//正常1 故障0
         bool guide_workcommand = 0;//开工1 停工0
         bool guide_takeovercommand = 1; //进入接管1    退出接管0
         bool guide_timeout = 0;    //超时1  未超时0
         int guide_task = 0;   //收到任务1    完成任务2   取消任务3
-        bool guide_nearpath = 1; //靠近路线1   远离路线0
+        bool guide_nearpath = 0; //靠近路线1   远离路线0
+        bool guide_mapfinish = 0;  //到达终点1  未达终点0
         double guide_amilng = 0.0;
         double guide_amilat = 0.0;
-        //==============================================================
+        int guide_lastcarState = 5;//进入云平台人工接管前的状态 0:待命停车 1:任务中 2:返程 3:返库 4:停工停车
+        //====================================================================================
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_jsguide/decition/adc_tools/compute_00.cpp

@@ -494,6 +494,11 @@ int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<G
             minDis = tmpdis;
             maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
             maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+            ServiceCarStatus.guide_nearpath = 1;//===========apollo_fu 2022 ==========
+        }
+        else//===========apollo_fu 2022 ==========
+        {
+            ServiceCarStatus.guide_nearpath = 0;//===========apollo_fu 2022 ==========
         }
     }
 
@@ -535,6 +540,11 @@ int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<
             minDis = tmpdis;
             maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
             maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+            ServiceCarStatus.guide_nearpath = 1;//===========apollo_fu 2022 ==========
+        }
+        else//===========apollo_fu 2022 ==========
+        {
+            ServiceCarStatus.guide_nearpath = 0;//===========apollo_fu 2022 ==========
         }
     }
 

src/decition/decition_brain_sf_jsguide/decition/brain.cpp

@@ -29,7 +29,14 @@ namespace iv {
 namespace decition {
 iv::decition::BrainDecition * gbrain;
 
+//======================================= apollo_fu 2022 ===================================
+void ListenRemoteCtrl(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    gbrain->UpdateRemoteCtrl(strdata,nSize);
+    gbrain->UpdateSate();
+}
 
+//===========================================================================================
 void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     gbrain->UpdateMap(strdata,nSize);
@@ -154,7 +161,7 @@ iv::decition::BrainDecition::BrainDecition()
 
     //============================= apollo_fu 2022 ============================
     guide_pTimer = new QTimer();
-    guide_pTimer->setInterval(4000);//4s
+    guide_pTimer->setInterval(4000);//4s  备用
     //=========================================================================
 
 }
@@ -1002,55 +1009,60 @@ void iv::decition::BrainDecition::run() {
         //==============================apollo_fu 2022=====================================
 
 #if 0
-        int guide_carState = 4;	// 0:待命停车 1:任务中	 2:返程 3:返库 4:停工停车 5:人工接管
+        int guide_carState = 5;	// 0:待命停车 1:任务中	 2:返程 3:返库 4:停工停车 5:云平台人工接管
         bool guide_faultstate = 1;//正常1 故障0
         bool guide_workcommand = 0;//开工1 停工0
         bool guide_takeovercommand = 1; //进入接管1    退出接管0
         bool guide_timeout = 0;    //超时1  未超时0
         int guide_task = 0;   //收到任务1    完成任务2   取消任务3
+        bool guide_nearpath = 0; //靠近路线1   远离路线0
+        bool guide_mapfinish = 0;  //到达终点1  未达终点0
         double guide_amilng = 0.0;
         double guide_amilat = 0.0;
+        int guide_lastcarState = 5;//进入云平台人工接管前的状态 0:待命停车 1:任务中 2:返程 3:返库 4:停工停车
 
 #endif
         switch (ServiceCarStatus.guide_carState) {
         case 0:
-            if(ServiceCarStatus.guide_task == 1)
+            if(ServiceCarStatus.guide_task == 1)//收到任务（目的地）
             {
                 ServiceCarStatus.guide_carState = 1;
             }
-            if(ServiceCarStatus.guide_timeout == 1)
+            if(ServiceCarStatus.guide_timeout == 1)//待命超时
             {
                 ServiceCarStatus.guide_carState = 2;
             }
-            if(ServiceCarStatus.guide_workcommand == 0)
+            if(ServiceCarStatus.guide_workcommand == 0)//停工指令
             {
                 ServiceCarStatus.guide_carState = 3;
             }
-            if(ServiceCarStatus.guide_faultstate == 0)
+            if(ServiceCarStatus.guide_faultstate == 0)//出现故障
             {
                 ServiceCarStatus.guide_carState = 4;
             }
-            if(ServiceCarStatus.guide_takeovercommand == 0)
+            if(ServiceCarStatus.guide_takeovercommand == 1)//接管指令
             {
                 ServiceCarStatus.guide_carState = 5;
+                ServiceCarStatus.guide_lastcarState = 0;
             }
             break;
         case 1:
-            if(ServiceCarStatus.guide_task == 2 || ServiceCarStatus.guide_task == 3)
+            if(ServiceCarStatus.guide_task == 2 || ServiceCarStatus.guide_task == 3)  //完成/取消任务
             {
                 ServiceCarStatus.guide_carState = 0;
             }
-            if(ServiceCarStatus.guide_workcommand == 0)
+            if(ServiceCarStatus.guide_workcommand == 0)//停工指令
             {
                 ServiceCarStatus.guide_carState = 3;
             }
-            if(ServiceCarStatus.guide_faultstate == 0)
+            if(ServiceCarStatus.guide_faultstate == 0)//出现故障
             {
                 ServiceCarStatus.guide_carState = 4;
             }
-            if(ServiceCarStatus.guide_takeovercommand == 0)
+            if(ServiceCarStatus.guide_takeovercommand == 1)//接管指令
             {
                 ServiceCarStatus.guide_carState = 5;
+                ServiceCarStatus.guide_lastcarState = 1;
             }
             break;
         case 2:
@@ -1058,15 +1070,63 @@ void iv::decition::BrainDecition::run() {
             {
                 ServiceCarStatus.guide_carState = 0;
             }
+            if(ServiceCarStatus.guide_workcommand == 0)//停工指令
+            {
+                ServiceCarStatus.guide_carState = 3;
+            }
+            if(ServiceCarStatus.guide_faultstate == 0)//出现故障
+            {
+                ServiceCarStatus.guide_carState = 4;
+            }
+            if(ServiceCarStatus.guide_takeovercommand == 1)//接管指令
+            {
+                ServiceCarStatus.guide_carState = 5;
+                ServiceCarStatus.guide_lastcarState = 2;
+            }
             break;
         case 3:
-
+            if(ServiceCarStatus.guide_faultstate == 0 || 1)  //出现故障  ||  需要添加 到达维护位置
+            {
+                ServiceCarStatus.guide_carState = 4;
+            }
+            if(ServiceCarStatus.guide_takeovercommand == 1)  //接管指令
+            {
+                ServiceCarStatus.guide_carState = 5;
+                ServiceCarStatus.guide_lastcarState = 3;
+            }
             break;
         case 4:
-
+            if(ServiceCarStatus.guide_workcommand == 1)//开工指令
+            {
+                ServiceCarStatus.guide_carState = 0;
+            }
+            if(ServiceCarStatus.guide_takeovercommand == 1)//接管指令
+            {
+                ServiceCarStatus.guide_carState = 5;
+                ServiceCarStatus.guide_lastcarState = 4;
+            }
             break;
         case 5:
-
+            if(ServiceCarStatus.guide_takeovercommand == 0 && ServiceCarStatus.guide_nearpath == 0)//退出接管 && 远离路线
+            {
+                ServiceCarStatus.guide_carState = 0;
+            }
+            if(ServiceCarStatus.guide_takeovercommand == 0 && ServiceCarStatus.guide_lastcarState == 1 && ServiceCarStatus.guide_nearpath == 1)//退出接管 && 曾在任务状态 && 靠近路线
+            {
+                ServiceCarStatus.guide_carState = 1;
+            }
+            if(ServiceCarStatus.guide_takeovercommand == 0 && ServiceCarStatus.guide_lastcarState == 2 && ServiceCarStatus.guide_nearpath == 1)//退出接管 && 曾在返程状态 && 靠近路线
+            {
+                ServiceCarStatus.guide_carState = 2;
+            }
+            if(ServiceCarStatus.guide_takeovercommand == 0 && ServiceCarStatus.guide_lastcarState == 3 && ServiceCarStatus.guide_nearpath == 1)//退出接管 && 曾在返库状态 && 靠近路线
+            {
+                ServiceCarStatus.guide_carState = 3;
+            }
+            if(ServiceCarStatus.guide_takeovercommand == 0 && ServiceCarStatus.guide_lastcarState == 4)//退出接管 && 曾在停工状态
+            {
+                ServiceCarStatus.guide_carState = 4;
+            }
             break;
         default:
             break;
@@ -1702,3 +1762,10 @@ void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
     ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
     ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
 }
+
+//======================================= apollo_fu 2022 ===================================
+void iv::decition::BrainDecition::UpdateRemoteCtrl(const char *pdata, const int ndatasize)
+{
+
+}
+//===========================================================================================

src/decition/decition_brain_sf_jsguide/decition/brain.h

@@ -162,6 +162,7 @@ namespace iv {
 
 
         public:
+            void UpdateRemoteCtrl(const char * pdata,const int ndatasize);//=========== apollo_fu 2022 =========
             void UpdateHMI(const char * pdata,const int ndatasize);
             void UpdatePlatform(const char * pdata,const int ndatasize);
             void UpdateGroupInfo(const char * pdata,const int ndatasize);

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -1053,8 +1053,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         }
 
                         if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = -0.5;
+                        ServiceCarStatus.guide_mapfinish = 1;//===========apollo_fu 2022 ==========
                 }
+                ServiceCarStatus.guide_mapfinish = 0;//===========apollo_fu 2022 ==========
     }else{
+        ServiceCarStatus.guide_mapfinish = 0;//===========apollo_fu 2022 ==========
 //                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
 //                        minDecelerate=-0.5;
 //                        std::cout<<"map finish brake"<<std::endl;