fix(grpc_BS):add fsm logic.not test

src/driver/driver_cloud_grpc_client_BS/VehicleControl.proto

@@ -80,7 +80,7 @@ message UploadMapReply {
 message CtrlModeReply {
     string id = 1; // 车辆 SIM 码
     CtrlMode modeCMD = 2; //mode change command
-    UseStatus useStatusCMD = 3;//使用状态修改命令 变量名修改 by Samuel
+    UseStatus useStatusCMD = 3; //使用状态修改命令
     GPSPoint deactivatePosition = 4; //停用站点
     double speedCMD = 5; //平台设定的期望运行速度
     NavSwitch navagationSwitch = 6; //开始-停止导航

src/driver/driver_cloud_grpc_client_BS/boost.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.pro

@@ -20,6 +20,8 @@ unix:system("./protomake.sh")
 
 SOURCES += \
         ../../common/common/math/gnss_coordinate_convert.cpp \
+        ../../include/msgtype/FSMSkipCMD.pb.cc \
+        ../../include/msgtype/brainstate.pb.cc \
         VehicleControl.grpc.pb.cc \
         VehicleControl.pb.cc \
         VehicleControl_service.grpc.pb.cc \
@@ -32,6 +34,7 @@ SOURCES += \
         VehicleUpload.pb.cc \
         VehicleUpload_service.grpc.pb.cc \
         VehicleUpload_service.pb.cc \
+        fsm_unit.cpp \
         main.cpp \
         vehicle_control.cpp \
         vehicle_patrol.cpp \
@@ -40,10 +43,7 @@ SOURCES += \
         ../../include/msgtype/chassis.pb.cc \
         ../../include/msgtype/gpsimu.pb.cc \
         ../../include/msgtype/platform_feedback.pb.cc \
-        ../../include/msgtype/rawpic.pb.cc \
-        ../../include/msgtype/startturnstile.pb.cc \
-        ../../include/msgtype/turnstile.pb.cc
-
+        ../../include/msgtype/rawpic.pb.cc
 
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin
@@ -70,9 +70,10 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
     error( "Couldn't find the ivyaml-cpp.pri file!" )
 }
 
-
 HEADERS += \
         ../../common/common/math/gnss_coordinate_convert.h \
+        ../../include/msgtype/FSMSkipCMD.pb.h \
+        ../../include/msgtype/brainstate.pb.h \
         VehicleControl.grpc.pb.h \
         VehicleControl.pb.h \
         VehicleControl_service.grpc.pb.h \
@@ -85,6 +86,8 @@ HEADERS += \
         VehicleUpload.pb.h \
         VehicleUpload_service.grpc.pb.h \
         VehicleUpload_service.pb.h \
+        boost.h \
+        fsm_unit.h \
         gps_type.h \
         vehicle_control.h \
         vehicle_patrol.h \
@@ -93,8 +96,6 @@ HEADERS += \
         ../../include/msgtype/chassis.pb.h \
         ../../include/msgtype/gpsimu.pb.h \
         ../../include/msgtype/platform_feedback.pb.h \
-        ../../include/msgtype/rawpic.pb.h \
-        ../../include/msgtype/startturnstile.pb.h \
-        ../../include/msgtype/turnstile.pb.h
+        ../../include/msgtype/rawpic.pb.h
 
 #before compile in ubuntu20.04 : sudo apt install libyaml-cpp-dev libgrpc++-dev

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.yaml

@@ -14,6 +14,14 @@ vehicleType : 0 #0 跑腿 1 导览 2 清扫
 speedWantAvg : 4.2 # m/s the speed used for calculate arrival time
 waitTime : 600 # s wait time after arriving
 
+FSMSkipCMDinterval : 100 #ms
+waitingStation:
+  latitude: 39.000000
+  longitude: 117.000000
+maintainStation:
+  latitude: 39.000000
+  longitude: 117.000000
+
 pic_front:
   msgname: picfront
   buffersize: 10000000
@@ -62,4 +70,12 @@ xodr_request:
   msgname: xodrreq
   buffersize: 1000
   buffercount: 1
+brain_state:
+  msgname: brainstate
+  buffersize: 10000
+  buffercount: 1
+FSM_skip_command:
+  msgname: FSMSkipCommand
+  buffersize: 10000
+  buffercount: 1
 

src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

@@ -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;
+        }
+    }
+}

src/driver/driver_cloud_grpc_client_BS/fsm_unit.h

@@ -0,0 +1,100 @@
+#ifndef FSMUNIT_H
+#define FSMUNIT_H
+
+#include <QObject>
+#include <QTimer>
+#include <QDateTime>
+#include <QMutex>
+
+#include <yaml-cpp/yaml.h>
+
+#include "modulecomm.h"
+#include "FSMSkipCMD.pb.h"
+#include "brainstate.pb.h"
+
+#ifndef IV_MSGUNIT
+#define IV_MSGUNIT
+
+namespace iv {
+struct msgunit
+{
+    char mstrmsgname[256];
+    int mnBufferSize = 10000;
+    int mnBufferCount = 1;
+    void * mpa = nullptr;
+    std::shared_ptr<char> mpstrmsgdata;
+    int mndatasize = 0;
+    bool mbRefresh = false;
+    bool mbImportant = false;
+    int mnkeeptime = 100;
+};
+}
+
+#endif
+
+#ifndef GLOBAL_GPS_POINT
+#define GLOBAL_GPS_POINT
+struct gGPSPoint
+{
+    double latitude = 0.0;
+    double longitude = 0.0;
+    double height = 0.0;
+};
+#endif
+
+class FSMUnit : public QObject
+{
+    Q_OBJECT
+
+public:
+    FSMUnit(void);
+
+    ~FSMUnit(void);
+
+    void dec_yaml(const char * stryamlpath);
+
+    void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenBrainStateMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+private:
+    unsigned int skipCMDSendInterval = 100;
+    gGPSPoint waitingStation;
+    gGPSPoint maintainStation;
+
+    iv::msgunit shmGPSIMU;
+    iv::msgunit shmFSMSkipCMD;
+    iv::msgunit shmBrainState;
+
+    QMutex mutex_GPSIMU;
+    gGPSPoint currentPosition;
+    double currentSpeed;
+
+    gGPSPoint currentDestination;
+    QTimer arriveCheckLoopTimer;
+
+    QTimer skipCMDSendTimer;
+    uint32_t FSMState = 0;
+    iv::brain::MissionCMD missionCMD = iv::brain::MissionCMD::MISSION_RESERVED; // 任务开始 完成 取消
+    iv::brain::WorkCMD workCMD = iv::brain::WorkCMD::WORK_RESERVED; // 开工 停工
+    iv::brain::RemoteCtrlCMD remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED; // 人工接管进入 退出
+    bool waitingStationArrived = false; // 到达等待站点
+    bool maintainStationArrived = false; // 到达维护站点
+
+signals:
+    void refreshFSMStatus(void);
+    void useStatusChanged(bool status);
+    void setAllowPlan(bool isAllow);
+
+public slots:
+    void navagationSwitchChanged_Slot(bool status);
+    void useStatusCMDChanged_Slot(bool status);
+    void remoteCtrlModeChanged_Slot(bool status);
+    void setFSMDestination_Slot(double latitude,double longitude);
+
+private slots:
+    void skipCMDSend_Timeout(void);
+    void refreshFSMStatus_Slot(void);
+    void arriveCheckLoop_Timeout(void);
+};
+
+#endif // FSMUNIT_H

src/driver/driver_cloud_grpc_client_BS/gps_type.h

@@ -75,15 +75,13 @@ namespace iv {
         int mnreserved[5];
         double mfreserved[2];
 
-
-
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
      class Point2D
     {
       public:
-          double x = 0, y = 0, speed=0;
+         double x = 0, y = 0, speed=0;
          int v1 = 0, v2 = 0;
 
          Point2D()
@@ -95,12 +93,6 @@ namespace iv {
         {
             x = _x; y = _y;
         }
-
-
      };
-
-
-
-
 }
 #endif // !_IV_COMMON_GPS_TYPE_

src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -10,6 +10,7 @@
 #include "vehicle_upload.h"
 #include "vehicle_control.h"
 #include "vehicle_patrol.h"
+#include "fsm_unit.h"
 
 #include "ivversion.h"
 #include "modulecomm.h"
@@ -204,5 +205,13 @@ int main(int argc, char *argv[])
 //    VehiclePatrolExceptionClient vehiclepatrol(grpc::CreateChannel(patrol_str, grpc::InsecureChannelCredentials()));
 //    vehiclepatrol.start();
 
+    FSMUnit *fsmunit = new FSMUnit();
+    QObject::connect(fsmunit,&FSMUnit::setAllowPlan,vehicleuploadmap,&VehicleUploadMapClient::setAllowPlan_Slot);
+    QObject::connect(fsmunit,&FSMUnit::useStatusChanged,vehicleuploaddata,&DataExchangeClient::useStatusChanged_Slot);
+    QObject::connect(vehicleuploaddata,&DataExchangeClient::setFSMDestination,fsmunit,&FSMUnit::setFSMDestination_Slot);
+    QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::remoteCtrlModeChanged,fsmunit,&FSMUnit::remoteCtrlModeChanged_Slot);
+    QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::navagationSwitchChanged,fsmunit,&FSMUnit::navagationSwitchChanged_Slot);
+    QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::useStatusCMDChanged,fsmunit,&FSMUnit::useStatusCMDChanged_Slot);
+
     return a.exec();
 }

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -272,7 +272,7 @@ std::string VehicleChangeCtrlModeClient::changeCtrlMode(void)
     if (status.ok()) {
         if(reply.id() == gstrid)
         {
-            modeCMD = reply.modecmd();
+            modeChangeCMD = reply.modecmd();
             useStatusCMD = reply.usestatuscmd();//使用状态修改命令
             deactivatePosition = reply.deactivateposition(); //停用站点 等号重载了copyFrom
             speedCMD = reply.speedcmd(); //平台设定的期望运行速度
@@ -286,7 +286,7 @@ std::string VehicleChangeCtrlModeClient::changeCtrlMode(void)
         {
             std::cout << "vehicleControl RPC connect timeout" << std::endl;
         }
-        modeCMD = CtrlMode::CMD_EMERGENCY_STOP;
+        modeChangeCMD = CtrlMode::CMD_EMERGENCY_STOP;
         return "changeCtrlMode RPC failed";
     }
 }
@@ -294,9 +294,24 @@ std::string VehicleChangeCtrlModeClient::changeCtrlMode(void)
 void VehicleChangeCtrlModeClient::updateCtrolMode(void)
 {
 //    std::cout<<"modeCMD:"<<modeCMD<<std::endl;
+    if(modeChangeCMD == org::jeecg::defsControl::grpc::CtrlMode::CMD_AUTO)
+        emit remoteCtrlModeChanged(false);
+    else
+        emit remoteCtrlModeChanged(true);
+
+    if(navagationSwitch == org::jeecg::defsControl::grpc::NavSwitch::NAV_START)
+        emit navagationSwitchChanged(true);
+    else
+        emit navagationSwitchChanged(false);
+
+    if(useStatusCMD == org::jeecg::defsControl::grpc::UseStatus::ENABLING)
+        emit useStatusCMDChanged(true);
+    else
+        emit useStatusCMDChanged(false);
+
     gNavagationSwitch = navagationSwitch;
-    gCtrlMode_Status = modeCMD;
-    emit ctrlMode_Changed(modeCMD);
+    gCtrlMode_Status = modeChangeCMD;
+    emit ctrlMode_Changed(modeChangeCMD);
 }
 
 void VehicleChangeCtrlModeClient::run()
@@ -413,7 +428,7 @@ void VehicleUploadMapClient::run()
             {
                 std::string reply = uploadMap();
 //                std::cout<< reply <<std::endl;
-                if(isNeedMap == true)
+                if(isNeedMap == true && allowPlan == true)
                 {
                     updateMapPOIData();
                 }
@@ -435,3 +450,8 @@ void VehicleUploadMapClient::uploadPath_Finished_Slot(std::string pathID)
         isNeedMap = false;
     }
 }
+
+void VehicleUploadMapClient::setAllowPlan_Slot(bool isAllow)
+{
+    allowPlan = isAllow;
+}

src/driver/driver_cloud_grpc_client_BS/vehicle_control.h

@@ -95,7 +95,7 @@ protected:
 private:
     std::unique_ptr<VehicleControl::Stub> stub_;
 
-    org::jeecg::defsControl::grpc::CtrlMode modeCMD = org::jeecg::defsControl::grpc::CtrlMode::CMD_AUTO;
+    org::jeecg::defsControl::grpc::CtrlMode modeChangeCMD = org::jeecg::defsControl::grpc::CtrlMode::CMD_AUTO;
     org::jeecg::defsControl::grpc::UseStatus useStatusCMD = org::jeecg::defsControl::grpc::UseStatus::ENABLING;//使用状态修改命令
     org::jeecg::defsControl::grpc::GPSPoint deactivatePosition; //停用站点
     double speedCMD = 0.0; //平台设定的期望运行速度
@@ -103,6 +103,9 @@ private:
 
 signals:
     void ctrlMode_Changed(org::jeecg::defsControl::grpc::CtrlMode ctrlMode);
+    void remoteCtrlModeChanged(bool status);
+    void navagationSwitchChanged(bool status);
+    void useStatusCMDChanged(bool status);
 };
 
 class VehicleUploadMapClient : public QThread{
@@ -126,11 +129,14 @@ private:
     std::string patrolPathID;
     QVector<org::jeecg::defsControl::grpc::MapPoint> POIPoints;
 
+    bool allowPlan = true;
+
 signals:
     void patrolPOI_Recieved(std::string pathID);
 
 public slots:
     void uploadPath_Finished_Slot(std::string pathID);
+    void setAllowPlan_Slot(bool isAllow);
 };
 
 #endif // VEHICLE_CONTROL_H

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.cpp

@@ -217,38 +217,6 @@ void VehiclePatrolExceptionClient::ListenGPSIMUMsg(const char * strdata,const un
     currentPosition.set_height(xdata.height());
     currentSpeed = xdata.speed();
     mutex_GPSIMU.unlock();
-
-}
-
-void VehiclePatrolExceptionClient::ListenTurnstileMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
-{
-    iv::vision::turnstile xdata;
-    if(!xdata.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<" ListenTurnstileMsg parese error."<<std::endl;
-        return;
-    }
-
-    if(statusTSGM == 1)
-    {
-        std::cout<<"Get Turnstile result"<<std::endl;
-        //set TSGM resul
-        isTSGM = true;
-        if(xdata.state() == false)
-            gateStatus = 1; //0 no gate 1 gate close 2 gate open
-        else
-            gateStatus = 2;
-        std::cout<<"gateStatus : "<<(int)gateStatus<<std::endl;
-        gateImage.clear();
-        gateImage.append(xdata.pic().picdata().data(),xdata.pic().picdata().size());
-        gateTime = QDateTime::currentMSecsSinceEpoch(); //time when get gateImage
-        mutex_GPSIMU.lock();
-        gatePosition.CopyFrom(currentPosition); //positon when get gateImage
-        mutex_GPSIMU.unlock();
-
-        timerTSGM.restart();
-        statusTSGM = 2;
-    }
 }
 
 std::string VehiclePatrolExceptionClient::uploadVehiclePatrolInfo(void)
@@ -367,107 +335,11 @@ void VehiclePatrolExceptionClient::run()
     xTime.start();
     int lastTime = xTime.elapsed();
     uint64_t interval = std::atoi(gstrpatrolInterval.c_str());
-    org::jeecg::defsPatrol::grpc::GPSPoint tempPosition;
-    //set isNeedTSGM
-    isNeedTSGM = true;
-    //set gateDestination
-    gateDestination.set_latitude(39.0665855);
-    gateDestination.set_longitude(117.3554362);
-    timerTSGM.start();
+
     while (!QThread::isInterruptionRequested())
     {
         if(abs(xTime.elapsed() - lastTime)>=interval)
         {
-            // do something
-            mutex_GPSIMU.lock();
-            tempPosition.CopyFrom(currentPosition);
-            double tempSpeed = currentSpeed;
-            mutex_GPSIMU.unlock();
-
-            if(fabs(tempPosition.latitude()-gateDestination.latitude()) < 0.0001 && fabs(tempPosition.longitude()-gateDestination.longitude()) < 0.0001)
-            {
-                if(tempSpeed < 0.1 && statusTSGM == 0 && isNeedTSGM == true)
-                {
-                    std::cout<<"Send startTurnstile"<<std::endl;
-                    iv::vision::startturnstile xmsg;
-
-                    xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
-                    xmsg.set_cameraname(shmPicRight.mstrmsgname);
-                    xmsg.set_start(true);
-
-                    int ndatasize = xmsg.ByteSize();
-                    char * str = new char[ndatasize];
-                    std::shared_ptr<char> pstr;pstr.reset(str);
-                    if(!xmsg.SerializeToArray(str,ndatasize))
-                    {
-                        std::cout<<"StartTurnstile serialize error."<<std::endl;
-                        return;
-                    }
-                    iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);
-                    statusTSGM = 1;
-                    timerTSGM.restart();
-                }
-            }
-
-            if(statusTSGM == 1 && timerTSGM.elapsed() > 80000)
-            {
-                //timeout
-                std::cout<<"Turnstile timeout"<<std::endl;
-                iv::vision::startturnstile xmsg;
-
-                xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
-                xmsg.set_cameraname(shmPicLeft.mstrmsgname);
-                xmsg.set_start(false);
-
-                int ndatasize = xmsg.ByteSize();
-                char * str = new char[ndatasize];
-                std::shared_ptr<char> pstr;pstr.reset(str);
-                if(!xmsg.SerializeToArray(str,ndatasize))
-                {
-                    std::cout<<"StartTurnstile serialize error."<<std::endl;
-                    return;
-                }
-                iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);
-
-                isTSGM = false;
-                gateStatus = 0;
-                gateImage.clear();
-                gateTime = QDateTime::currentMSecsSinceEpoch();
-                mutex_GPSIMU.lock();
-                gatePosition.CopyFrom(currentPosition);
-                mutex_GPSIMU.unlock();
-
-                timerTSGM.restart();
-                statusTSGM = 3;
-            }
-
-            if(statusTSGM == 2 || statusTSGM == 3)
-            {
-                if(fabs(tempPosition.latitude()-gateDestination.latitude()) > 0.0001 || fabs(tempPosition.longitude()-gateDestination.longitude()) > 0.0001)
-                {
-                    std::cout<<"Turnstile detect finish"<<std::endl;
-                    iv::vision::startturnstile xmsg;
-
-                    xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
-                    xmsg.set_cameraname(shmPicLeft.mstrmsgname);
-                    xmsg.set_start(false);
-
-                    int ndatasize = xmsg.ByteSize();
-                    char * str = new char[ndatasize];
-                    std::shared_ptr<char> pstr;pstr.reset(str);
-                    if(!xmsg.SerializeToArray(str,ndatasize))
-                    {
-                        std::cout<<"StartTurnstile serialize error."<<std::endl;
-                        return;
-                    }
-                    iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);
-
-                    timerTSGM.restart();
-                    statusTSGM = 0;
-                }
-            }
-
-
 //            updatePatrolData();
 //            std::string reply = uploadVehiclePatrolInfo();
 //            std::cout<< reply <<std::endl;

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.h

@@ -60,7 +60,6 @@ public:
     void dec_yaml(const char * stryamlpath);
 
     void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    void ListenTurnstileMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
 protected:
     void run();

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -740,7 +740,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     sensorStatusCamLeft = false;
     sensorStatusCamRight = false;
 
-    useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+//    useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
     if(useStatus == org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING)
     {
         destinationSaved.Clear();
@@ -771,6 +771,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
             isArrived = 2;
             remainPathLength = 0.0;
             destinationSaved.Clear();
+            emit setFSMDestination(destinationSaved.latitude(),destinationSaved.latitude());
             std::cout<<"destination has arrived"<<std::endl;
         }
     }
@@ -912,6 +913,7 @@ void DataExchangeClient::path_Updated_Slot(void)
     destinationSaved.set_latitude(pathPoints.at(pathPoints.size()-1).mappoint().latitude());
     destinationSaved.set_longitude(pathPoints.at(pathPoints.size()-1).mappoint().longitude());
     destinationSaved.set_height(0.0);
+    emit setFSMDestination(destinationSaved.latitude(),destinationSaved.latitude());
     remainPathLength = totalPathLength;
     isArrived = 1; ///< 0 no path 1 not arrived 2 arrived
     uploadPath();
@@ -922,8 +924,17 @@ void DataExchangeClient::destination_Refreshed_Timeout(void)
     if(destinationRefreshedTimer.isActive() == true)
         destinationRefreshedTimer.stop();
     destinationSaved.Clear();
+    emit setFSMDestination(destinationSaved.latitude(),destinationSaved.latitude());
     remainPathLength = 0.0;
     isArrived = 0;
     emit uploadPath_Finished("");
     std::cout<<"path plan timeout."<<std::endl;
 }
+
+void DataExchangeClient::useStatusChanged_Slot(bool status)
+{
+    if(status)
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+    else
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING;
+}

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -177,9 +177,13 @@ signals:
     void uploadPath_Finished(std::string pathID);
     void destination_Recieved(void);
     void path_Updated(void);
+    void setFSMDestination(double latitude,double longitude);
 
 public slots:
     void patrolPOI_Recieved_Slot(std::string pathID);
+    void useStatusChanged_Slot(bool status);
+
+private slots:
     void destination_Recieved_Slot(void);
     void path_Updated_Slot(void);
     void destination_Refreshed_Timeout(void);