add fsm logic

src/decition/common/common/car_status.h

@@ -244,13 +244,34 @@ namespace iv {
         int guide_lastcarState = 5;//进入云平台人工接管前的状态 0:待命停车 1:任务中 2:返程 3:返库 4:停工停车
         //====================================================================================
 
+       // bool mbReceiveTask=false; //js 的车辆增加状态机
+       // bool mbFulfilTask=false;
+        //bool mbCloseToPath =false;
+        bool mbAwayFromPath=false;
+       // int  mnReceiveTaskCnt=0;
+       bool mbNoPath=false;
         int mbNoPathCnt=0;
-        bool mbNoPath=false;
-
 
         //超声波信息
         //std::vector<iv::ultrasonic::Area> mbUltraArea;
         float mbUltraDis[4]={200,200,200,200};
+        //状态机信息，beangin
+        int mbFSM_State=0; //0:待命停车   1:任务中	 2:人工接管 3:停工停车 4:返程 5:返库
+        int mbLastFSM_State=0;
+        //接收共享内存其他模块传递出来的信息作状态机条件跳变
+        bool mbErrState = 0;//1出现故障，0 无故障,chassis
+        bool mbStartStopWork = 2;//开工指令1 停工指令0
+        bool mbTakeOverEn = 2; //进入接管1    退出接管0
+        bool mbOvertimeEn = 0;    //超时1  未超时0
+        int mbOvertimeCnt=0;//计数到一定数目还没收到任务，就记为超时
+        int mbTaskCmdType = 0;   //收到任务1    完成任务2   取消任务3
+        bool mbCloseToPath = 2; //靠近路线1   远离路线0
+        bool mbFullFillTask = 2;  //到达终点1  未达终点0
+        bool mbArriveInitPos=0;//到达初始位置1,未到达是0
+        bool mbArriveServicePos=0;//到达维护位置1,未到达是0
+
+//        double guide_amilng = 0.0;
+//        double guide_amilat = 0.0;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_jsguide/decition/brain.cpp

@@ -74,6 +74,11 @@ void ListenUltraArea(const char *strdata, const unsigned int nSize, const unsign
     gbrain->UpdateUltra(strdata,nSize); //超声波数据
 }
 
+void ListenFSMSkipCmd(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    gbrain->UpdateFSMSkipCmd(strdata,nSize); //状态机跳转命令
+}
+
 /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             iv::formation_map_index::map map;
@@ -162,6 +167,8 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpUltraArea = iv::modulecomm::RegisterRecv("ultra_area",iv::decition::ListenUltraArea);
 
+    mpFSMSkipCmd = iv::modulecomm::RegisterRecv("FSMSkipCmmand",iv::decition::ListenFSMSkipCmd);
+
 
     mTime.start();
     mnOldTime = mTime.elapsed();
@@ -500,6 +507,208 @@ void iv::decition::BrainDecition::run() {
 
     while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
     {
+        //有限状态机逻辑，begin
+       if(ServiceCarStatus.mbFSM_State==0)//待命
+        {
+             //待命停车
+                if(ServiceCarStatus.mbOvertimeCnt<=100)//待命计时
+                {
+                    ServiceCarStatus.mbOvertimeCnt++;
+                }
+                else
+                {
+                    ServiceCarStatus.mbOvertimeEn=1;
+                }
+                //
+                if(ServiceCarStatus.mbErrState == 1)//出现故障
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 3;
+                    ServiceCarStatus.mbLastFSM_State=0;
+                }
+                //
+                if(ServiceCarStatus.mbStartStopWork == 0)//停工指令
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 5;
+                    ServiceCarStatus.mbLastFSM_State=0;
+                }
+                //
+                if(ServiceCarStatus.mbOvertimeEn == 1)//待命超时
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 4;
+                    ServiceCarStatus.mbLastFSM_State=0;
+                }
+                //
+                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 2;
+                    ServiceCarStatus.mbLastFSM_State = 0;
+                }
+                //
+                if(ServiceCarStatus.mbTaskCmdType == 1)//收到任务（目的地）
+                {
+                    ServiceCarStatus.mbRunPause=false; //true车辆不动，false车辆动
+                    ServiceCarStatus.mbFSM_State = 1;
+                    ServiceCarStatus.mbLastFSM_State=0;
+                }
+       }
+       else if(ServiceCarStatus.mbFSM_State==1)
+        {
+                //
+                if(ServiceCarStatus.mbTaskCmdType == 2 || ServiceCarStatus.mbTaskCmdType == 3)  //完成/取消任务
+                {
+                    ServiceCarStatus.mbRunPause=true;
+                    ServiceCarStatus.mbOvertimeEn=0;
+                    ServiceCarStatus.mbOvertimeCnt=0;
+                    ServiceCarStatus.mbFSM_State = 0;
+                    ServiceCarStatus.mbLastFSM_State=1;
+                }
+                //
+                if(ServiceCarStatus.mbStartStopWork == 0)//停工指令
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 5;
+                    ServiceCarStatus.mbLastFSM_State=1;
+                }
+                //
+                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 2;
+                    ServiceCarStatus.mbLastFSM_State=1;
+                }
+                //
+                if(ServiceCarStatus.mbErrState ==1)//出现故障
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 3;
+                    ServiceCarStatus.mbLastFSM_State=1;
+                }
+         }
+       else if(ServiceCarStatus.mbFSM_State==2)
+        {
+                //ServiceCarStatus.mbRunPause=true;
+            //
+                if((ServiceCarStatus.mbTakeOverEn == 0)&& (ServiceCarStatus.mbLastFSM_State == 3))//退出接管 && 曾在停工状态
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 3;
+                    ServiceCarStatus.mbLastFSM_State=2;
+                }
+                //
+                if((ServiceCarStatus.mbTakeOverEn == 0)&& (ServiceCarStatus.mbLastFSM_State == 5)
+                        &&(ServiceCarStatus.mbCloseToPath == 1))
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 5;
+                    ServiceCarStatus.mbLastFSM_State=2;
+                }
+                //
+                if((ServiceCarStatus.mbTakeOverEn == 0)&& (ServiceCarStatus.mbLastFSM_State == 4)
+                        &&(ServiceCarStatus.mbCloseToPath == 1))
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 4;
+                    ServiceCarStatus.mbLastFSM_State=2;
+                }
+                if((ServiceCarStatus.mbTakeOverEn == 0) && (ServiceCarStatus.mbLastFSM_State == 0)
+                        &&(ServiceCarStatus.mbCloseToPath == 0))//退出接管 && 远离路线
+                {
+                    ServiceCarStatus.mbRunPause=true;
+                    ServiceCarStatus.mbFSM_State = 0;
+                    ServiceCarStatus.mbOvertimeEn=0;
+                    ServiceCarStatus.mbOvertimeCnt=0;
+                    ServiceCarStatus.mbLastFSM_State=2;
+                }
+                if((ServiceCarStatus.mbTakeOverEn == 0) && (ServiceCarStatus.mbLastFSM_State == 1)
+                        &&(ServiceCarStatus.mbCloseToPath == 1))//退出接管 && 远离路线
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 1;
+                    ServiceCarStatus.mbLastFSM_State=2;
+                }
+          }
+       else if(ServiceCarStatus.mbFSM_State==3)
+        {
+                if(ServiceCarStatus.mbStartStopWork == 1)//开工指令
+                {
+                    ServiceCarStatus.mbRunPause=true;
+                    ServiceCarStatus.mbOvertimeEn=0;
+                    ServiceCarStatus.mbOvertimeCnt=0;
+                    ServiceCarStatus.mbFSM_State = 0;
+                    ServiceCarStatus.mbLastFSM_State=3;
+                }
+                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 2;
+                    ServiceCarStatus.mbLastFSM_State=3;
+                }
+        }
+       else if(ServiceCarStatus.mbFSM_State==4)
+        {
+            //
+                if(ServiceCarStatus.mbLastFSM_State==0)
+                {
+                    ServiceCarStatus.mbOvertimeEn=0;
+                    ServiceCarStatus.mbOvertimeCnt=0;
+                }
+                 //
+                if(ServiceCarStatus.mbErrState ==1)//出现故障
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 3;
+                    ServiceCarStatus.mbLastFSM_State=4;
+                }
+                //
+                if(ServiceCarStatus.mbArriveInitPos == 1)
+                {
+                    ServiceCarStatus.mbRunPause=true;
+                    ServiceCarStatus.mbFSM_State = 0;
+                    ServiceCarStatus.mbOvertimeEn=0;
+                    ServiceCarStatus.mbOvertimeCnt=0;
+                    ServiceCarStatus.mbLastFSM_State=4;
+                }
+                //
+                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 2;
+                    ServiceCarStatus.mbLastFSM_State=4;
+                }
+                //
+                if(ServiceCarStatus.mbStartStopWork == 0)//
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 5;
+                    ServiceCarStatus.mbLastFSM_State=4;
+                }
+          }
+       else if(ServiceCarStatus.mbFSM_State==5)
+        {
+            //
+                if((ServiceCarStatus.mbArriveServicePos == 1)||(ServiceCarStatus.mbErrState == 1))//退出接管 && 曾在停工状态
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 3;
+                    ServiceCarStatus.mbLastFSM_State=5;
+                }
+                //
+                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                {
+                    ServiceCarStatus.mbRunPause=false;
+                    ServiceCarStatus.mbFSM_State = 2;
+                    ServiceCarStatus.mbLastFSM_State=5;
+                }
+        }
+       else
+       {
+        ;
+       }
+        //有限状态机逻辑，end
         if(navigation_data.size() <1)
         {
             iv::map::mapreq x;
@@ -554,6 +763,8 @@ void iv::decition::BrainDecition::run() {
             xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
             xbs.set_mfobs(ServiceCarStatus.mObs);
             xbs.set_decision_period(decision_period);
+            xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
+            xbs.set_mbfsm_state(ServiceCarStatus.mbFSM_State);
             ShareBrainstate(xbs);    //apollo_fu 20200413 qudiaozhushi
             //            iv::decition::VehicleStateBasic vsb;
             //            vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
@@ -893,6 +1104,8 @@ void iv::decition::BrainDecition::run() {
             xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
             xbs.set_mfobs(ServiceCarStatus.mObs);
             xbs.set_decision_period(decision_period);
+            xbs.set_mbfsm_state(ServiceCarStatus.mbFSM_State);
+            xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
             ShareBrainstate(xbs);
 
 
@@ -1029,133 +1242,6 @@ void iv::decition::BrainDecition::run() {
         iv::modulecomm::ModuleSendMsg(mpaToPlatform,(char*)&toplat,sizeof(iv::platform::PlatFormMsg));
         //ODS("\ngetSensor时长:%f\n", end1 - start1);
 
-        //==============================apollo_fu 2022=====================================
-
-#if 0
-        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)//收到任务（目的地）
-            {
-                ServiceCarStatus.guide_carState = 1;
-            }
-            if(ServiceCarStatus.guide_timeout == 1)//待命超时
-            {
-                ServiceCarStatus.guide_carState = 2;
-            }
-            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 = 0;
-            }
-            break;
-        case 1:
-            if(ServiceCarStatus.guide_task == 2 || ServiceCarStatus.guide_task == 3)  //完成/取消任务
-            {
-                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 = 1;
-            }
-            break;
-        case 2:
-            if(0)  //需要添加 到达初始待命位置
-            {
-                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;
-        }
-        //==================================================================================
-
     }
     std::cout << "\n\n\n\n\n\n\n\n\n\n\nbrain线程退出\n" << std::endl;
 }
@@ -1814,6 +1900,10 @@ void iv::decition::BrainDecition::UpdateUltra(const char *pdata, const int ndata
         }
     }
 
+}
+
+void iv::decition::BrainDecition::UpdateFSMSkipCmd(const char *pdata, const int ndatasize)
+{
 
 }
 void iv::decition::BrainDecition::adjuseGpsLidarPosition(){

src/decition/decition_brain_sf_jsguide/decition/brain.h

@@ -143,6 +143,7 @@ namespace iv {
             void * mpaObsTraceRight;
             void * mpaCarstate;
             void * mpUltraArea;
+            void * mpFSMSkipCmd;
 
 
             void ShareDecition(iv::decition::Decition decition);
@@ -173,6 +174,8 @@ namespace iv {
             void GetFusion(const char* pdata, const int ndatasize);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
             void UpdateUltra(const char * pdata,const int ndatasize);
+            void UpdateFSMSkipCmd(const char * pdata,const int ndatasize);
+
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
             iv::LidarGridPtr fusion_ptr_ = NULL;

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -351,6 +351,13 @@ 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);
     }
@@ -1022,12 +1029,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if (PathPoint<0)
     {
-
+        ServiceCarStatus.mbCloseToPath=0;
+        ServiceCarStatus.mbAwayFromPath = 1;
         minDecelerate=-1.0;
         phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
         return gps_decition;
 
     }
+    else
+    {
+        ServiceCarStatus.mbCloseToPath=1;
+        ServiceCarStatus.mbAwayFromPath = 0;
+    }
 
     DecideGps00::lastGpsIndex = PathPoint;
 
@@ -2378,6 +2391,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
          }
      }
   //超声波逻辑,end
+
+     //状体机逻辑
+     if((ServiceCarStatus.mbTaskCmdType == 3)||(ServiceCarStatus.mbErrState ==1)
+             ||(ServiceCarStatus.mbTakeOverEn == 1))//收到取消任务/接管/故障后车辆停止
+     {
+         minDecelerate=min(-0.3f,minDecelerate);
+     }
+
     if(ServiceCarStatus.group_control){
         dSpeed = ServiceCarStatus.group_comm_speed;
     }

src/include/proto/brainstate.proto

@@ -10,4 +10,12 @@ 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;
+
+  optional uint32 mbFSM_State=12; //0:待命停车   1:任务中	 2:人工接管 3:停工停车 4:返程 5:返库
 };