FSM SKIP logic

src/decition/decition_brain_sf_jsguide/decition/brain.cpp

@@ -510,6 +510,7 @@ void iv::decition::BrainDecition::run() {
         //有限状态机逻辑，begin
        if(ServiceCarStatus.mbFSM_State==0)//待命
         {
+                ServiceCarStatus.mbRunPause=true;
              //待命停车
                 if(ServiceCarStatus.mbOvertimeCnt<=100)//待命计时
                 {
@@ -520,7 +521,7 @@ void iv::decition::BrainDecition::run() {
                     ServiceCarStatus.mbOvertimeEn=1;
                 }
                 //
-                if(ServiceCarStatus.mbErrState == 1)//出现故障
+                if(ServiceCarStatus.mbErrState == 1&&ServiceCarStatus.speed==0.0)//出现故障
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 3;
@@ -541,7 +542,7 @@ void iv::decition::BrainDecition::run() {
                     ServiceCarStatus.mbLastFSM_State=0;
                 }
                 //
-                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 2;
@@ -558,7 +559,7 @@ void iv::decition::BrainDecition::run() {
        else if(ServiceCarStatus.mbFSM_State==1)
         {
                 //
-                if(ServiceCarStatus.mbTaskCmdType == 2 || ServiceCarStatus.mbTaskCmdType == 3)  //完成/取消任务
+                if((ServiceCarStatus.mbTaskCmdType == 2 || ServiceCarStatus.mbTaskCmdType == 3)&&ServiceCarStatus.speed==0.0)  //完成/取消任务
                 {
                     ServiceCarStatus.mbRunPause=true;
                     ServiceCarStatus.mbOvertimeEn=0;
@@ -574,14 +575,14 @@ void iv::decition::BrainDecition::run() {
                     ServiceCarStatus.mbLastFSM_State=1;
                 }
                 //
-                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 2;
                     ServiceCarStatus.mbLastFSM_State=1;
                 }
                 //
-                if(ServiceCarStatus.mbErrState ==1)//出现故障
+                if(ServiceCarStatus.mbErrState ==1&&ServiceCarStatus.speed==0.0)//出现故障
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 3;
@@ -641,7 +642,7 @@ void iv::decition::BrainDecition::run() {
                     ServiceCarStatus.mbFSM_State = 0;
                     ServiceCarStatus.mbLastFSM_State=3;
                 }
-                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 2;
@@ -657,14 +658,14 @@ void iv::decition::BrainDecition::run() {
                     ServiceCarStatus.mbOvertimeCnt=0;
                 }
                  //
-                if(ServiceCarStatus.mbErrState ==1)//出现故障
+                if(ServiceCarStatus.mbErrState ==1&&ServiceCarStatus.speed==0.0)//出现故障
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 3;
                     ServiceCarStatus.mbLastFSM_State=4;
                 }
                 //
-                if(ServiceCarStatus.mbArriveInitPos == 1)
+                if(ServiceCarStatus.mbArriveInitPos == 1&&ServiceCarStatus.speed==0.0)
                 {
                     ServiceCarStatus.mbRunPause=true;
                     ServiceCarStatus.mbFSM_State = 0;
@@ -673,7 +674,7 @@ void iv::decition::BrainDecition::run() {
                     ServiceCarStatus.mbLastFSM_State=4;
                 }
                 //
-                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 2;
@@ -697,7 +698,7 @@ void iv::decition::BrainDecition::run() {
                     ServiceCarStatus.mbLastFSM_State=5;
                 }
                 //
-                if(ServiceCarStatus.mbTakeOverEn == 1)//接管指令
+                if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
                 {
                     ServiceCarStatus.mbRunPause=false;
                     ServiceCarStatus.mbFSM_State = 2;
@@ -1904,7 +1905,18 @@ void iv::decition::BrainDecition::UpdateUltra(const char *pdata, const int ndata
 
 void iv::decition::BrainDecition::UpdateFSMSkipCmd(const char *pdata, const int ndatasize)
 {
-
+    if(ndatasize<1)return;
+    iv::brain::fsm_skip_cmd fsm_cmd;
+    if(false == fsm_cmd.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Update FSM Skip CMD Parse fail."<<std::endl;
+        return;
+    }
+    ServiceCarStatus.mbTaskCmdType = fsm_cmd.missioncmd();
+    ServiceCarStatus.mbStartStopWork=fsm_cmd.workcmd();
+    ServiceCarStatus.mbTakeOverEn = fsm_cmd.remotectrlcmd();
+    ServiceCarStatus.mbArriveInitPos=fsm_cmd.waitingstationarrived();
+    ServiceCarStatus.mbArriveServicePos=fsm_cmd.maintainstationarrived();
 }
 void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
 

src/decition/decition_brain_sf_jsguide/decition/brain.h

@@ -48,6 +48,7 @@
 #include "carstate.pb.h"
 #include "groupmsg.pb.h"
 #include "ultraarea.pb.h"
+#include "FSMSkipCMD.pb.h"
 
 #include "fanyaapi.h"
 

src/decition/decition_brain_sf_jsguide/decition_brain_sf_jsguide.pro

@@ -35,6 +35,7 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/carstate.pb.cc \
     ../../include/msgtype/groupmsg.pb.cc \
    ../../include/msgtype/ultraarea.pb.cc \
+   ../../include/msgtype/FSMSkipCMD.pb.cc \
     ../../include/msgtype/remotectrl.pb.cc
 
 
@@ -101,6 +102,7 @@ HEADERS += \
     ../../include/msgtype/carstate.pb.h \
     ../../include/msgtype/groupmsg.pb.h \
    ../../include/msgtype/ultraarea.pb.h \
+   ../../include/msgtype/FSMSkipCMD.pb.h \
     ../../include/msgtype/remotectrl.pb.h
 
 

src/include/proto/FSMSkipCMD.proto

@@ -0,0 +1,35 @@
+syntax = "proto2";
+
+package iv.brain;
+
+enum MissionCMD
+{
+MISSION_RESERVED = 0;
+MISSION_START = 1;
+MISSIOH_FINISH = 2;
+MISSION_CANCEL = 3;
+}
+
+enum WorkCMD
+{
+WORK_STOP = 0;
+WORK_START = 1;
+WORK_RESERVED = 2;
+}
+
+enum RemoteCtrlCMD
+{
+REMOTE_CTRL_EXIT = 0;
+REMOTE_CTRL_ENTER = 1;
+REMOTE_CTRL_RESERVED = 2;
+}
+
+message fsm_skip_cmd
+{
+required int64 timestamp = 1;
+optional MissionCMD missionCMD = 2; // 任务开始 完成 取消
+optional WorkCMD workCMD = 3; // 开工 停工
+optional RemoteCtrlCMD remoteCtrlCMD = 4; // 人工接管进入 退出
+optional bool waitingStationArrived = 7; // 到达等待站点
+optional bool maintainStationArrived = 8; // 到达维护站点
+}