|
@@ -67,6 +67,11 @@ iv::decition::BrainDecition * gbrain;
|
|
|
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;
|
|
@@ -155,6 +160,7 @@ iv::decition::BrainDecition::BrainDecition()
|
|
|
|
|
|
mpultraArea=iv::modulecomm::RegisterRecv("ultra_area",iv::decition::ListenUltraArea);//监听超声波数据
|
|
|
|
|
|
+ mpFSMSkipCmd = iv::modulecomm::RegisterRecv("FSMSkipCommand",iv::decition::ListenFSMSkipCmd);
|
|
|
|
|
|
mTime.start();
|
|
|
mnOldTime = mTime.elapsed();
|
|
@@ -472,6 +478,208 @@ void iv::decition::BrainDecition::run() {
|
|
|
|
|
|
while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
|
|
|
{
|
|
|
+ //有限状态机逻辑,begin
|
|
|
+ if(ServiceCarStatus.mbFSM_State==0)//待命
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mbRunPause=true;
|
|
|
+ //待命停车
|
|
|
+ if(ServiceCarStatus.mbOvertimeCnt<=3000)//待命计时
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mbOvertimeCnt++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mbOvertimeEn=1;
|
|
|
+ }
|
|
|
+ //
|
|
|
+ if(ServiceCarStatus.mbErrState == 1&&ServiceCarStatus.speed==0.0)//出现故障
|
|
|
+ {
|
|
|
+ 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.speed==0.0)//接管指令
|
|
|
+ {
|
|
|
+ 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.speed==0.0) //完成/取消任务
|
|
|
+ {
|
|
|
+ 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.speed==0.0)//接管指令
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mbRunPause=false;
|
|
|
+ ServiceCarStatus.mbFSM_State = 2;
|
|
|
+ ServiceCarStatus.mbLastFSM_State=1;
|
|
|
+ }
|
|
|
+ //
|
|
|
+ if(ServiceCarStatus.mbErrState ==1&&ServiceCarStatus.speed==0.0)//出现故障
|
|
|
+ {
|
|
|
+ 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.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.speed==0.0)//接管指令
|
|
|
+ {
|
|
|
+ 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.speed==0.0)//出现故障
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mbRunPause=false;
|
|
|
+ ServiceCarStatus.mbFSM_State = 3;
|
|
|
+ ServiceCarStatus.mbLastFSM_State=4;
|
|
|
+ }
|
|
|
+ //
|
|
|
+ if(ServiceCarStatus.mbArriveInitPos == 1&&ServiceCarStatus.speed==0.0)
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mbRunPause=true;
|
|
|
+ ServiceCarStatus.mbFSM_State = 0;
|
|
|
+ ServiceCarStatus.mbOvertimeEn=0;
|
|
|
+ ServiceCarStatus.mbOvertimeCnt=0;
|
|
|
+ ServiceCarStatus.mbLastFSM_State=4;
|
|
|
+ }
|
|
|
+ //
|
|
|
+ if(ServiceCarStatus.mbTakeOverEn == 1&&ServiceCarStatus.speed==0.0)//接管指令
|
|
|
+ {
|
|
|
+ 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.speed==0.0)//接管指令
|
|
|
+ {
|
|
|
+ ServiceCarStatus.mbRunPause=false;
|
|
|
+ ServiceCarStatus.mbFSM_State = 2;
|
|
|
+ ServiceCarStatus.mbLastFSM_State=5;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ;
|
|
|
+ }
|
|
|
+ //有限状态机逻辑,end
|
|
|
if(navigation_data.size() <1)
|
|
|
{
|
|
|
iv::map::mapreq x;
|
|
@@ -489,7 +697,7 @@ void iv::decition::BrainDecition::run() {
|
|
|
delete str;
|
|
|
|
|
|
ServiceCarStatus.mbReceiveTask=false;
|
|
|
- ServiceCarStatus.mbFulfilTask = false;
|
|
|
+ ServiceCarStatus.mbFullFillTask = false;
|
|
|
ServiceCarStatus.mbCloseToPath = false;
|
|
|
ServiceCarStatus.mbAwayFromPath = false;
|
|
|
ServiceCarStatus.mnReceiveTaskCnt=0;
|
|
@@ -550,7 +758,7 @@ void iv::decition::BrainDecition::run() {
|
|
|
xbs.set_decision_period(decision_period);
|
|
|
xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
|
|
|
xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
|
|
|
- xbs.set_mbfulfiltask(ServiceCarStatus.mbFulfilTask);
|
|
|
+ xbs.set_mbfulfiltask(ServiceCarStatus.mbFullFillTask);
|
|
|
xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
|
|
|
xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
|
|
|
ShareBrainstate(xbs); //apollo_fu 20200413 qudiaozhushi
|
|
@@ -892,7 +1100,7 @@ void iv::decition::BrainDecition::run() {
|
|
|
xbs.set_decision_period(decision_period);
|
|
|
xbs.set_mbawayfrompath(ServiceCarStatus.mbAwayFromPath);
|
|
|
xbs.set_mbclosetopath(ServiceCarStatus.mbCloseToPath);
|
|
|
- xbs.set_mbfulfiltask(ServiceCarStatus.mbFulfilTask);
|
|
|
+ xbs.set_mbfulfiltask(ServiceCarStatus.mbFullFillTask);
|
|
|
xbs.set_mbreceivetask(ServiceCarStatus.mbReceiveTask);
|
|
|
xbs.set_mbnopath(ServiceCarStatus.mbNoPath);
|
|
|
ShareBrainstate(xbs);
|
|
@@ -1671,6 +1879,21 @@ 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(){
|
|
|
|