|
|
@@ -0,0 +1,357 @@
|
|
|
+#include "fsm_unit.h"
|
|
|
+
|
|
|
+#include "gpsimu.pb.h"
|
|
|
+#include "math/gnss_coordinate_convert.h"
|
|
|
+
|
|
|
+extern char stryamlpath[256];
|
|
|
+
|
|
|
+FSMUnit::FSMUnit(void)
|
|
|
+{
|
|
|
+ dec_yaml(stryamlpath);
|
|
|
+
|
|
|
+ shmFSMSkipCMD.mpa = iv::modulecomm::RegisterSend(shmFSMSkipCMD.mstrmsgname,shmFSMSkipCMD.mnBufferSize,shmFSMSkipCMD.mnBufferCount);
|
|
|
+ ModuleFun funupdate = std::bind(&FSMUnit::ListenBrainStateMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
|
|
|
+ shmBrainState.mpa = iv::modulecomm::RegisterRecvPlus(shmBrainState.mstrmsgname,funupdate);
|
|
|
+
|
|
|
+ QObject::connect(this,&FSMUnit::refreshFSMStatus,this,&FSMUnit::refreshFSMStatus_Slot);
|
|
|
+ QObject::connect(&skipCMDSendTimer,&QTimer::timeout,this,&FSMUnit::skipCMDSend_Timeout);
|
|
|
+ QObject::connect(&arriveCheckLoopTimer,&QTimer::timeout,this,&FSMUnit::arriveCheckLoop_Timeout);
|
|
|
+ if(skipCMDSendTimer.isActive() == false)skipCMDSendTimer.start(skipCMDSendInterval);
|
|
|
+ if(arriveCheckLoopTimer.isActive() == false)arriveCheckLoopTimer.start(100);
|
|
|
+}
|
|
|
+
|
|
|
+FSMUnit::~FSMUnit(void)
|
|
|
+{
|
|
|
+ if(shmFSMSkipCMD.mpa != nullptr)iv::modulecomm::Unregister(shmFSMSkipCMD.mpa);
|
|
|
+ if(shmBrainState.mpa != nullptr)iv::modulecomm::Unregister(shmBrainState.mpa);
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::dec_yaml(const char *stryamlpath)
|
|
|
+{
|
|
|
+ YAML::Node config;
|
|
|
+ try
|
|
|
+ {
|
|
|
+ config = YAML::LoadFile(stryamlpath);
|
|
|
+ }
|
|
|
+ catch(YAML::BadFile &e)
|
|
|
+ {
|
|
|
+ std::cout<<e.what()<<std::endl;
|
|
|
+ std::cout<<"yaml file load fail."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ catch(YAML::ParserException &e)
|
|
|
+ {
|
|
|
+ std::cout<<e.what()<<std::endl;
|
|
|
+ std::cout<<"yaml file is malformed."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::string strmsgname;
|
|
|
+
|
|
|
+ if(config["GPS_IMU"])
|
|
|
+ {
|
|
|
+ if(config["GPS_IMU"]["msgname"]&&config["GPS_IMU"]["buffersize"]&&config["GPS_IMU"]["buffercount"])
|
|
|
+ {
|
|
|
+ strmsgname = config["GPS_IMU"]["msgname"].as<std::string>();
|
|
|
+ strncpy(shmGPSIMU.mstrmsgname,strmsgname.data(),255);
|
|
|
+ shmGPSIMU.mnBufferSize = config["GPS_IMU"]["buffersize"].as<int>();
|
|
|
+ shmGPSIMU.mnBufferCount = config["GPS_IMU"]["buffercount"].as<int>();
|
|
|
+ std::cout << "GPS_IMU:" << shmGPSIMU.mstrmsgname << "," << shmGPSIMU.mnBufferSize << "," << shmGPSIMU.mnBufferCount << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ strmsgname = "hcp2_gpsimu";
|
|
|
+ strncpy(shmGPSIMU.mstrmsgname,strmsgname.data(),255);
|
|
|
+ shmGPSIMU.mnBufferSize = 10000;
|
|
|
+ shmGPSIMU.mnBufferCount = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(config["brain_state"])
|
|
|
+ {
|
|
|
+ if(config["brain_state"]["msgname"]&&config["brain_state"]["buffersize"]&&config["brain_state"]["buffercount"])
|
|
|
+ {
|
|
|
+ strmsgname = config["brain_state"]["msgname"].as<std::string>();
|
|
|
+ strncpy(shmBrainState.mstrmsgname,strmsgname.data(),255);
|
|
|
+ shmBrainState.mnBufferSize = config["brain_state"]["buffersize"].as<int>();
|
|
|
+ shmBrainState.mnBufferCount = config["brain_state"]["buffercount"].as<int>();
|
|
|
+ std::cout << "brain_state:" << shmBrainState.mstrmsgname << "," << shmBrainState.mnBufferSize << "," << shmBrainState.mnBufferCount << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ strmsgname = "brainstate";
|
|
|
+ strncpy(shmBrainState.mstrmsgname,strmsgname.data(),255);
|
|
|
+ shmBrainState.mnBufferSize = 10000;
|
|
|
+ shmBrainState.mnBufferCount = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(config["FSM_skip_command"])
|
|
|
+ {
|
|
|
+ if(config["FSM_skip_command"]["msgname"]&&config["FSM_skip_command"]["buffersize"]&&config["FSM_skip_command"]["buffercount"])
|
|
|
+ {
|
|
|
+ strmsgname = config["FSM_skip_command"]["msgname"].as<std::string>();
|
|
|
+ strncpy(shmFSMSkipCMD.mstrmsgname,strmsgname.data(),255);
|
|
|
+ shmFSMSkipCMD.mnBufferSize = config["FSM_skip_command"]["buffersize"].as<int>();
|
|
|
+ shmFSMSkipCMD.mnBufferCount = config["FSM_skip_command"]["buffercount"].as<int>();
|
|
|
+ std::cout << "FSM_skip_command:" << shmFSMSkipCMD.mstrmsgname << "," << shmFSMSkipCMD.mnBufferSize << "," << shmFSMSkipCMD.mnBufferCount << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ strmsgname = "FSMSkipCommand";
|
|
|
+ strncpy(shmFSMSkipCMD.mstrmsgname,strmsgname.data(),255);
|
|
|
+ shmFSMSkipCMD.mnBufferSize = 10000;
|
|
|
+ shmFSMSkipCMD.mnBufferCount = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(config["FSMSkipCMDinterval"])
|
|
|
+ {
|
|
|
+ skipCMDSendInterval = config["FSMSkipCMDinterval"].as<int>();
|
|
|
+ std::cout<<"FSMSkipCMDinterval:"<<skipCMDSendInterval<<std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(config["waitingStation"])
|
|
|
+ {
|
|
|
+ if(config["waitingStation"]["latitude"]&&config["waitingStation"]["longitude"])
|
|
|
+ {
|
|
|
+ waitingStation.latitude = config["waitingStation"]["latitude"].as<double>();
|
|
|
+ waitingStation.longitude = config["waitingStation"]["longitude"].as<double>();
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(config["maintainStation"])
|
|
|
+ {
|
|
|
+ if(config["maintainStation"]["latitude"]&&config["maintainStation"]["longitude"])
|
|
|
+ {
|
|
|
+ maintainStation.latitude = config["maintainStation"]["latitude"].as<double>();
|
|
|
+ maintainStation.longitude = config["maintainStation"]["longitude"].as<double>();
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return;
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
|
|
|
+{
|
|
|
+ iv::gps::gpsimu xdata;
|
|
|
+ if(!xdata.ParseFromArray(strdata,nSize))
|
|
|
+ {
|
|
|
+ std::cout<<" ListenGPSIMUMsg parese error."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+// double speed = sqrt(xdata.ve()*xdata.ve() + xdata.vn()*xdata.vn() + xdata.vd()*xdata.vd());
|
|
|
+
|
|
|
+ mutex_GPSIMU.lock();
|
|
|
+ currentPosition.latitude = xdata.lat();
|
|
|
+ currentPosition.longitude = xdata.lon();
|
|
|
+ currentPosition.height = xdata.height();
|
|
|
+ currentSpeed = xdata.speed();
|
|
|
+ mutex_GPSIMU.unlock();
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::ListenBrainStateMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+{
|
|
|
+ iv::brain::brainstate xdata;
|
|
|
+ if(!xdata.ParseFromArray(strdata,nSize))
|
|
|
+ {
|
|
|
+ std::cout<<" ListenBrainStateMsg parese error."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ FSMState = xdata.mbfsm_state(); //0:待命停车 1:任务中 2:人工接管 3:停工停车 4:返程 5:返库
|
|
|
+
|
|
|
+ emit refreshFSMStatus();
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::navagationSwitchChanged_Slot(bool status)
|
|
|
+{
|
|
|
+ if(status)
|
|
|
+ missionCMD = iv::brain::MissionCMD::MISSION_START;
|
|
|
+ else
|
|
|
+ missionCMD = iv::brain::MissionCMD::MISSION_CANCEL;
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::useStatusCMDChanged_Slot(bool status)
|
|
|
+{
|
|
|
+ if(status)
|
|
|
+ workCMD = iv::brain::WorkCMD::WORK_START;
|
|
|
+ else
|
|
|
+ workCMD = iv::brain::WorkCMD::WORK_STOP;
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::remoteCtrlModeChanged_Slot(bool status)
|
|
|
+{
|
|
|
+ if(status)
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER;
|
|
|
+ else
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT;
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::setFSMDestination_Slot(double latitude,double longitude)
|
|
|
+{
|
|
|
+ if(FSMState == 0)
|
|
|
+ {
|
|
|
+ currentDestination.latitude = latitude;
|
|
|
+ currentDestination.longitude = longitude;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ currentDestination.latitude = 0.0;
|
|
|
+ currentDestination.longitude = 0.0;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::skipCMDSend_Timeout(void)
|
|
|
+{
|
|
|
+ iv::brain::fsm_skip_cmd xmsg;
|
|
|
+ xmsg.set_timestamp(QDateTime::currentMSecsSinceEpoch());
|
|
|
+ xmsg.set_missioncmd(missionCMD);
|
|
|
+ xmsg.set_workcmd(workCMD);
|
|
|
+ xmsg.set_remotectrlcmd(remoteCtrlCMD);
|
|
|
+ xmsg.set_waitingstationarrived(waitingStationArrived);
|
|
|
+ xmsg.set_maintainstationarrived(maintainStationArrived);
|
|
|
+
|
|
|
+ int ndatasize = xmsg.ByteSize();
|
|
|
+ char * str = new char[ndatasize];
|
|
|
+ std::shared_ptr<char> pstr;pstr.reset(str);
|
|
|
+ if(!xmsg.SerializeToArray(str,ndatasize))
|
|
|
+ {
|
|
|
+ std::cout<<"FSMSkipCommand serialize error."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ iv::modulecomm::ModuleSendMsg(shmFSMSkipCMD.mpa,str,ndatasize);
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::refreshFSMStatus_Slot(void)
|
|
|
+{
|
|
|
+ switch (FSMState) {
|
|
|
+ case 0: ///< 0待命停车
|
|
|
+ if(missionCMD == iv::brain::MissionCMD::MISSIOH_FINISH \
|
|
|
+ || missionCMD == iv::brain::MissionCMD::MISSION_CANCEL)
|
|
|
+ {
|
|
|
+ missionCMD = iv::brain::MissionCMD::MISSION_RESERVED;
|
|
|
+ }
|
|
|
+ if(workCMD == iv::brain::WorkCMD::WORK_START)
|
|
|
+ {
|
|
|
+ workCMD = iv::brain::WorkCMD::WORK_RESERVED;
|
|
|
+ }
|
|
|
+ if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT)
|
|
|
+ {
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
|
|
|
+ }
|
|
|
+ if(waitingStationArrived == true)
|
|
|
+ {
|
|
|
+ waitingStationArrived = false;
|
|
|
+ }
|
|
|
+ emit useStatusChanged(true);
|
|
|
+ emit setAllowPlan(true);
|
|
|
+ break;
|
|
|
+ case 1: ///< 1任务中
|
|
|
+ if(missionCMD == iv::brain::MissionCMD::MISSION_START)
|
|
|
+ {
|
|
|
+ missionCMD = iv::brain::MissionCMD::MISSION_RESERVED;
|
|
|
+ }
|
|
|
+ if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT)
|
|
|
+ {
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
|
|
|
+ }
|
|
|
+ emit setAllowPlan(false);
|
|
|
+ break;
|
|
|
+ case 2: ///< 2人工接管
|
|
|
+ if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER)
|
|
|
+ {
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
|
|
|
+ }
|
|
|
+ emit setAllowPlan(false);
|
|
|
+ break;
|
|
|
+ case 3: ///< 3停工停车
|
|
|
+ if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT)
|
|
|
+ {
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
|
|
|
+ }
|
|
|
+ if(maintainStationArrived == true)
|
|
|
+ {
|
|
|
+ maintainStationArrived = false;
|
|
|
+ }
|
|
|
+ emit useStatusChanged(false);
|
|
|
+ emit setAllowPlan(false);
|
|
|
+ break;
|
|
|
+ case 4: ///< 4返程
|
|
|
+ if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT)
|
|
|
+ {
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
|
|
|
+ }
|
|
|
+ emit setAllowPlan(false);
|
|
|
+ break;
|
|
|
+ case 5: ///< 5返库
|
|
|
+ if(workCMD == iv::brain::WorkCMD::WORK_STOP)
|
|
|
+ {
|
|
|
+ workCMD = iv::brain::WorkCMD::WORK_RESERVED;
|
|
|
+ }
|
|
|
+ if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT)
|
|
|
+ {
|
|
|
+ remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
|
|
|
+ }
|
|
|
+ emit setAllowPlan(false);
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void FSMUnit::arriveCheckLoop_Timeout(void)
|
|
|
+{
|
|
|
+ if(FSMState == 1) ///< 1任务中
|
|
|
+ {
|
|
|
+ double localPositionX = 0.0;
|
|
|
+ double localPositionNextX = 0.0;
|
|
|
+ double localPositionY = 0.0;
|
|
|
+ double localPositionNextY = 0.0;
|
|
|
+ GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
|
|
|
+ GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
|
|
|
+ double deltaX = localPositionNextX - localPositionX;
|
|
|
+ double deltaY = localPositionNextY - localPositionY;
|
|
|
+ double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
|
|
|
+
|
|
|
+ if(distance < 1.5 && currentSpeed < 0.1)
|
|
|
+ {
|
|
|
+ missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(FSMState == 4) ///< 4返程
|
|
|
+ {
|
|
|
+ double localPositionX = 0.0;
|
|
|
+ double localPositionNextX = 0.0;
|
|
|
+ double localPositionY = 0.0;
|
|
|
+ double localPositionNextY = 0.0;
|
|
|
+ GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
|
|
|
+ GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
|
|
|
+ double deltaX = localPositionNextX - localPositionX;
|
|
|
+ double deltaY = localPositionNextY - localPositionY;
|
|
|
+ double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
|
|
|
+
|
|
|
+ if(distance < 1.5 && currentSpeed < 0.1)
|
|
|
+ {
|
|
|
+ waitingStationArrived = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(FSMState == 5) ///< 5返库
|
|
|
+ {
|
|
|
+ double localPositionX = 0.0;
|
|
|
+ double localPositionNextX = 0.0;
|
|
|
+ double localPositionY = 0.0;
|
|
|
+ double localPositionNextY = 0.0;
|
|
|
+ GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
|
|
|
+ GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
|
|
|
+ double deltaX = localPositionNextX - localPositionX;
|
|
|
+ double deltaY = localPositionNextY - localPositionY;
|
|
|
+ double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
|
|
|
+
|
|
|
+ if(distance < 1.5 && currentSpeed < 0.1)
|
|
|
+ {
|
|
|
+ maintainStationArrived = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
孙嘉城