fix(grpc_BS):fix fsm logic about work stop.

src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

@@ -1,5 +1,8 @@
 #include "fsm_unit.h"
 
+#include <math.h>
+#include <iomanip>
+
 #include "gpsimu.pb.h"
 #include "math/gnss_coordinate_convert.h"
 
@@ -10,8 +13,12 @@ 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);
@@ -23,7 +30,9 @@ FSMUnit::FSMUnit(void)
 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)
@@ -105,6 +114,25 @@ void FSMUnit::dec_yaml(const char *stryamlpath)
         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>();
@@ -161,6 +189,7 @@ void FSMUnit::ListenBrainStateMsg(const char * strdata,const unsigned int nSize,
     }
 
     FSMState = xdata.mbfsm_state(); //0:待命停车 1:任务中 2:人工接管 3:停工停车 4:返程 5:返库
+//    std::cout<<"fsm status: "<<(unsigned int)FSMState<<std::endl;
 
     emit refreshFSMStatus();
 }
@@ -203,6 +232,13 @@ void FSMUnit::setFSMDestination_Slot(double latitude,double longitude)
     }
 }
 
+void FSMUnit::setMissionFinished_Slot(void)
+{
+    missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
+    currentDestination.latitude = 0.0;
+    currentDestination.longitude = 0.0;
+}
+
 void FSMUnit::skipCMDSend_Timeout(void)
 {
     iv::brain::fsm_skip_cmd xmsg;
@@ -261,7 +297,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         break;
     case 2: ///< 2人工接管
         if(remoteCtrlCMD == iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER)
-        {
+        {void sendPathPlanRequest(void);
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
         emit setAllowPlan(false);
@@ -303,25 +339,14 @@ void FSMUnit::refreshFSMStatus_Slot(void)
 
 void FSMUnit::arriveCheckLoop_Timeout(void)
 {
-    if(FSMState == 1) ///< 1任务中
+    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)
+        if(currentDestination.latitude != waitingStation.latitude || currentDestination.longitude != waitingStation.longitude)
         {
-            missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
+            currentDestination.latitude = waitingStation.latitude;
+            currentDestination.longitude = waitingStation.longitude;
+            emit sendPathPlanRequest(currentDestination.latitude,currentDestination.longitude);
         }
-    }
-    if(FSMState == 4) ///< 4返程
-    {
         double localPositionX = 0.0;
         double localPositionNextX = 0.0;
         double localPositionY = 0.0;
@@ -339,6 +364,12 @@ void FSMUnit::arriveCheckLoop_Timeout(void)
     }
     if(FSMState == 5) ///< 5返库
     {
+        if(currentDestination.latitude != maintainStation.latitude || currentDestination.longitude != maintainStation.longitude)
+        {
+            currentDestination.latitude = maintainStation.latitude;
+            currentDestination.longitude = maintainStation.longitude;
+            emit sendPathPlanRequest(currentDestination.latitude,currentDestination.longitude);
+        }
         double localPositionX = 0.0;
         double localPositionNextX = 0.0;
         double localPositionY = 0.0;
@@ -355,3 +386,14 @@ void FSMUnit::arriveCheckLoop_Timeout(void)
         }
     }
 }
+
+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;
+
+    iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
+}

src/driver/driver_cloud_grpc_client_BS/fsm_unit.h

@@ -6,6 +6,7 @@
 #include <QDateTime>
 #include <QMutex>
 
+#include <iostream>
 #include <yaml-cpp/yaml.h>
 
 #include "modulecomm.h"
@@ -42,6 +43,22 @@ struct gGPSPoint
 };
 #endif
 
+#ifndef IV_XODROBJ
+#define IV_XODROBJ
+
+class xodrobj
+{
+public:
+    double flatsrc;
+    double flonsrc;
+    double fhgdsrc;
+    double flat;
+    double flon;
+    int lane;
+};
+
+#endif
+
 class FSMUnit : public QObject
 {
     Q_OBJECT
@@ -64,6 +81,7 @@ private:
     iv::msgunit shmGPSIMU;
     iv::msgunit shmFSMSkipCMD;
     iv::msgunit shmBrainState;
+    iv::msgunit shmXodrRequest;
 
     QMutex mutex_GPSIMU;
     gGPSPoint currentPosition;
@@ -84,17 +102,20 @@ signals:
     void refreshFSMStatus(void);
     void useStatusChanged(bool status);
     void setAllowPlan(bool isAllow);
+    void sendPathPlanRequest(double latitude,double longitude);
 
 public slots:
     void navagationSwitchChanged_Slot(bool status);
     void useStatusCMDChanged_Slot(bool status);
     void remoteCtrlModeChanged_Slot(bool status);
     void setFSMDestination_Slot(double latitude,double longitude);
+    void setMissionFinished_Slot(void);
 
 private slots:
     void skipCMDSend_Timeout(void);
     void refreshFSMStatus_Slot(void);
     void arriveCheckLoop_Timeout(void);
+    void sendPathPlanRequest_Slot(double latitude,double longitude);
 };
 
 #endif // FSMUNIT_H

src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -209,6 +209,7 @@ int main(int argc, char *argv[])
     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(vehicleuploaddata,&DataExchangeClient::setMissionFinished,fsmunit,&FSMUnit::setMissionFinished_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);

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -308,6 +308,7 @@ void VehicleChangeCtrlModeClient::updateCtrolMode(void)
         emit useStatusCMDChanged(true);
     else
         emit useStatusCMDChanged(false);
+//    std::cout<<"useStatus: "<<(int)useStatusCMD<<std::endl;
 
     gNavagationSwitch = navagationSwitch;
     gCtrlMode_Status = modeChangeCMD;

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -771,7 +771,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
             isArrived = 2;
             remainPathLength = 0.0;
             destinationSaved.Clear();
-            emit setFSMDestination(destinationSaved.latitude(),destinationSaved.latitude());
+            emit setMissionFinished();
             std::cout<<"destination has arrived"<<std::endl;
         }
     }

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -53,6 +53,9 @@ struct simpletrace
 };
 }
 
+#ifndef IV_XODROBJ
+#define IV_XODROBJ
+
 class xodrobj
 {
 public:
@@ -64,6 +67,8 @@ public:
     int lane;
 };
 
+#endif
+
 using grpc::Channel;
 using grpc::ClientContext;
 using grpc::Status;
@@ -178,6 +183,7 @@ signals:
     void destination_Recieved(void);
     void path_Updated(void);
     void setFSMDestination(double latitude,double longitude);
+    void setMissionFinished(void);
 
 public slots:
     void patrolPOI_Recieved_Slot(std::string pathID);