modularization/src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

#include "fsm_unit.h"

#include <math.h>
#include <iomanip>

#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);
    shmXodrRequest.mpa = iv::modulecomm::RegisterSend(shmXodrRequest.mstrmsgname,shmXodrRequest.mnBufferSize,shmXodrRequest.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);
    funupdate = std::bind(&FSMUnit::ListenGPSIMUMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmGPSIMU.mpa = iv::modulecomm::RegisterRecvPlus(shmGPSIMU.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);
    QObject::connect(this,&FSMUnit::sendPathPlanRequest,this,&FSMUnit::sendPathPlanRequest_Slot);
    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(shmXodrRequest.mpa != nullptr)iv::modulecomm::Unregister(shmXodrRequest.mpa);
    if(shmBrainState.mpa != nullptr)iv::modulecomm::Unregister(shmBrainState.mpa);
    if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.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["xodr_request"])
    {
        if(config["xodr_request"]["msgname"]&&config["xodr_request"]["buffersize"]&&config["xodr_request"]["buffercount"])
        {
            strmsgname = config["xodr_request"]["msgname"].as<std::string>();
            strncpy(shmXodrRequest.mstrmsgname,strmsgname.data(),255);
            shmXodrRequest.mnBufferSize = config["xodr_request"]["buffersize"].as<int>();
            shmXodrRequest.mnBufferCount = config["xodr_request"]["buffercount"].as<int>();
            std::cout << "xodr_request:" << shmXodrRequest.mstrmsgname << "," << shmXodrRequest.mnBufferSize << "," << shmXodrRequest.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "xodrreq";
        strncpy(shmXodrRequest.mstrmsgname,strmsgname.data(),255);
        shmXodrRequest.mnBufferSize = 1000;
        shmXodrRequest.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:返库
//    std::cout<<"fsm status: "<<(unsigned int)FSMState<<std::endl;

    emit refreshFSMStatus();
}

void FSMUnit::navagationSwitchChanged_Slot(bool status)
{
    if(status == true && FSMState == 0)
    {
        missionCMD = iv::brain::MissionCMD::MISSION_START;
        std::cout<<"mission start"<<std::endl;
    }
    else if(status == false && FSMState == 1)
    {
        missionCMD = iv::brain::MissionCMD::MISSION_CANCEL;
        std::cout<<"mission canceled"<<std::endl;
    }
    else
        missionCMD = iv::brain::MissionCMD::MISSION_RESERVED;
}

void FSMUnit::useStatusCMDChanged_Slot(bool status)
{
    if(status == true && FSMState == 3)
        workCMD = iv::brain::WorkCMD::WORK_START;
    else if(status == false && (FSMState == 0 || FSMState == 1 || FSMState == 4))
        workCMD = iv::brain::WorkCMD::WORK_STOP;
    else
        workCMD = iv::brain::WorkCMD::WORK_RESERVED;
}

void FSMUnit::remoteCtrlModeChanged_Slot(bool status)
{
    if(status == true && FSMState != 2)
        remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER;
    else if(status == false && FSMState == 2)
        remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT;
    else
        remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
}

void FSMUnit::setFSMDestination_Slot(double latitude,double longitude)
{
    if(FSMState == 0 || FSMState == 1 || FSMState == 4 || FSMState == 5)
    {
        currentDestination.latitude = latitude;
        currentDestination.longitude = longitude;
    }
    else
    {
        currentDestination.latitude = 0.0;
        currentDestination.longitude = 0.0;
    }
}

void FSMUnit::setMissionFinished_Slot(void)
{
    if(FSMState == 1)
    {
        missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
        std::cout<<"mission finished"<<std::endl;
    }
    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(1);
        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 useStatusChanged(2);
        emit setAllowPlan(false);
        break;
    case 2: ///< 2人工接管
        if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER)
        {
            remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
        }
        emit useStatusChanged(3);
        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(0);
        emit setAllowPlan(false);
        break;
    case 4: ///< 4返程
        if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT)
        {
            remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
        }
        emit useStatusChanged(4);
        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 useStatusChanged(5);
        emit setAllowPlan(false);
        break;
    default:
        break;
    }
}

void FSMUnit::arriveCheckLoop_Timeout(void)
{
    if(FSMState == 4) ///< 4返程
    {

        double localPositionX = 0.0;
        double localPositionNextX = 0.0;
        double localPositionY = 0.0;
        double localPositionNextY = 0.0;
        GaussProjCal(waitingStation.longitude,waitingStation.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 > 5.0)
        {
            std::cout<<"destination miss distance in FSM mode 4:"<<distance<<std::endl;
            emit sendPathPlanRequest(waitingStation.latitude,waitingStation.longitude);
        }
        GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
        GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
        deltaX = localPositionNextX - localPositionX;
        deltaY = localPositionNextY - localPositionY;
        distance = sqrt(deltaX*deltaX + deltaY*deltaY);

        if(distance <= 5.0 && 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(maintainStation.longitude,maintainStation.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);
//        std::cout<<"distance"<<distance<<std::endl;

        if(distance > 5.0)
        {
            emit sendPathPlanRequest(maintainStation.latitude,maintainStation.longitude);
        }
        GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
        GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
        deltaX = localPositionNextX - localPositionX;
        deltaY = localPositionNextY - localPositionY;
        distance = sqrt(deltaX*deltaX + deltaY*deltaY);

        if(distance <= 5.0 && currentSpeed < 0.1)
        {
            maintainStationArrived = true;
        }
    }
}

void FSMUnit::sendPathPlanRequest_Slot(double latitude,double longitude)
{
    xodrobj xodrDest;
    xodrDest.flon = longitude;
    xodrDest.flat = latitude;
    xodrDest.lane = 1;
//    std::cout<<"current destination: "<<std::setprecision(12)<<longitude<<","<<latitude<<std::endl;
    std::cout<<"send a path request"<<std::endl;
    iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
}