fsm logic

src/detection/detection_chassis/main.cpp

@@ -128,7 +128,7 @@ int main(int argc, char *argv[])
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
     std::string strmemcan = xp.GetParam("canrecv","canrecv0");
-    std::string strvehicletype = xp.GetParam("vehicletype","GE3");
+    std::string strvehicletype = xp.GetParam("vehicletype","JSRunlegs");
     std::string strmodulename = xp.GetParam("modulename","chassis"); //chassismsgname
     std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");
 

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.pro

@@ -19,6 +19,7 @@ DEFINES += QT_DEPRECATED_WARNINGS
 unix:system("./protomake.sh")
 
 SOURCES += \
+        ../../common/common/math/gnss_coordinate_convert.cpp \
         VehicleControl.grpc.pb.cc \
         VehicleControl.pb.cc \
         VehicleControl_service.grpc.pb.cc \
@@ -71,6 +72,7 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 
 
 HEADERS += \
+        ../../common/common/math/gnss_coordinate_convert.h \
         VehicleControl.grpc.pb.h \
         VehicleControl.pb.h \
         VehicleControl_service.grpc.pb.h \

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.yaml

@@ -11,6 +11,8 @@ uploadmapinterval : 1000
 id : 1234567890123456789H
 plateNumber : 津A123456
 vehicleType : 0 #0 跑腿 1 导览 2 清扫
+speedWantAvg : 4.2 # m/s the speed used for calculate arrival time
+waitTime : 600 # s wait time after arriving
 
 pic_front:
   msgname: picfront

src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -49,6 +49,9 @@ std::string gstrid = "1234567890123456789H";
 std::string gstrplateNumber = "津A123456";
 std::string gvehicleType = "0"; //0 delevery 1 guid 2 clear
 
+double gspeedWantAvg = 4.1; // m/s
+double gwaitTime = 600; // s
+
 char stryamlpath[256];
 
 void dec_yaml(const char * stryamlpath)
@@ -128,6 +131,16 @@ void dec_yaml(const char * stryamlpath)
         gvehicleType = config["vehicleType"].as<std::string>();
         std::cout<<"vehicleType:"<<gstrplateNumber<<std::endl;
     }
+    if(config["speedWantAvg"])
+    {
+        gspeedWantAvg = config["speedWantAvg"].as<double>();
+        std::cout<<"speedWantAvg: "<<gspeedWantAvg<<" m/s"<<std::endl;
+    }
+    if(config["waitTime"])
+    {
+        gwaitTime = config["waitTime"].as<double>();
+        std::cout<<"waitTime: "<<gwaitTime<<std::endl;
+    }
 
     return;
 }
@@ -164,7 +177,7 @@ int main(int argc, char *argv[])
 //    std::cout<<control_str<<std::endl;
     VehicleChangeCtrlModeClient *vehiclechangectrlmode = new VehicleChangeCtrlModeClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
     VehicleControlClient *vehiclecontrol = new VehicleControlClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
-    qRegisterMetaType<org::jeecg::defsControl::grpc::CtrlMode>("org::jeecg::defsControl::grpc::CtrlMode"); ///< to solve the problem of signal and slot being in different threads
+    qRegisterMetaType<org::jeecg::defsControl::grpc::CtrlMode>("org::jeecg::defsControl::grpc::CtrlMode"); //< to solve the problem of signal and slot being in different threads
     QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::ctrlMode_Changed,vehiclecontrol,&VehicleControlClient::ctrlMode_Changed_Slot);
     vehiclechangectrlmode->start();
     vehiclecontrol->start();
@@ -176,8 +189,8 @@ int main(int argc, char *argv[])
     vehicleuploadmap->start();
 
 //    std::cout<<patrol_str<<std::endl;
-    VehiclePatrolExceptionClient vehiclepatrol(grpc::CreateChannel(patrol_str, grpc::InsecureChannelCredentials()));
-    vehiclepatrol.start();
+//    VehiclePatrolExceptionClient vehiclepatrol(grpc::CreateChannel(patrol_str, grpc::InsecureChannelCredentials()));
+//    vehiclepatrol.start();
 
     return a.exec();
 }

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -1,6 +1,7 @@
 #include "vehicle_control.h"
 
 #include <math.h>
+#include <thread>
 
 #include "modulecomm.h"
 #include "remotectrl.pb.h"
@@ -260,6 +261,10 @@ std::string VehicleChangeCtrlModeClient::changeCtrlMode(void)
         if(reply.id() == gstrid)
         {
             modeCMD = reply.modecmd();
+            useStatusCMD = reply.usestatuscmd();//使用状态修改命令
+            deactivatePosition = reply.deactivateposition(); //停用站点 等号重载了copyFrom
+            speedCMD = reply.speedcmd(); //平台设定的期望运行速度
+            navagationSwitch = reply.navagationswitch(); //开始-停止导航
         }
         return "changeCtrlMode RPC successed";
     } else {
@@ -345,9 +350,9 @@ std::string VehicleUploadMapClient::uploadMap(void)
             POIPoints.clear();
 
             isNeedMap = reply.isneedmap();
-//            std::cout<<reply.isneedmap()<<std::endl;
+//            std::cout<<"isneedmap: "<<reply.isneedmap()<<std::endl;
             patrolPathID = reply.patrolpathid();
-//            std::cout<<reply.patrolpathid()<<std::endl;
+//            std::cout<<"pathid: "<<reply.patrolpathid()<<std::endl;
             for(int i = 0;i < reply.mappoints_size();i++)
             {
                 POIPoints.append(reply.mappoints(i));

src/driver/driver_cloud_grpc_client_BS/vehicle_control.h

@@ -95,7 +95,11 @@ protected:
 private:
     std::unique_ptr<VehicleControl::Stub> stub_;
 
-    org::jeecg::defsControl::grpc::CtrlMode modeCMD = org::jeecg::defsControl::grpc::CtrlMode::CMD_EMERGENCY_STOP;
+    org::jeecg::defsControl::grpc::CtrlMode modeCMD = 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; //平台设定的期望运行速度
+    org::jeecg::defsControl::grpc::NavSwitch navagationSwitch = org::jeecg::defsControl::grpc::NavSwitch::NAV_STOP; //开始-停止导航
 
 signals:
     void ctrlMode_Changed(org::jeecg::defsControl::grpc::CtrlMode ctrlMode);

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.cpp

@@ -2,6 +2,7 @@
 
 #include <QFile>
 #include <math.h>
+#include <thread>
 
 #include "modulecomm.h"
 #include "gpsimu.pb.h"
@@ -28,10 +29,10 @@ VehiclePatrolExceptionClient::VehiclePatrolExceptionClient(std::shared_ptr<Chann
 
     ModuleFun funupdate = std::bind(&VehiclePatrolExceptionClient::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);
-    funupdate = std::bind(&VehiclePatrolExceptionClient::ListenTurnstileMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    shmTurnstile.mpa = iv::modulecomm::RegisterRecvPlus(shmTurnstile.mstrmsgname,funupdate);
+//    funupdate = std::bind(&VehiclePatrolExceptionClient::ListenTurnstileMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+//    shmTurnstile.mpa = iv::modulecomm::RegisterRecvPlus(shmTurnstile.mstrmsgname,funupdate);
 
-    shmStartTurnstile.mpa = iv::modulecomm::RegisterSend(shmStartTurnstile.mstrmsgname,shmStartTurnstile.mnBufferSize,shmStartTurnstile.mnBufferCount);
+//    shmStartTurnstile.mpa = iv::modulecomm::RegisterSend(shmStartTurnstile.mstrmsgname,shmStartTurnstile.mnBufferSize,shmStartTurnstile.mnBufferCount);
 }
 
 VehiclePatrolExceptionClient::~VehiclePatrolExceptionClient(void)

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.h

@@ -82,28 +82,28 @@ private:
 
     std::string id;
 
-    bool isTVR = false; //Traffic Violation Recognition
-    int32_t violationStatus; //0 no violation 1 overspeed 2 illegal parking 3 direction wrong 4 run the red light
+    bool isTVR = false; ///< Traffic Violation Recognition
+    int32_t violationStatus; ///< 0 no violation 1 overspeed 2 illegal parking 3 direction wrong 4 run the red light
     std::string vehicleLicenseNumber;
     QByteArray violationImage;
-    uint64_t violationTime; //time when get violationImage
-    org::jeecg::defsPatrol::grpc::GPSPoint violationPosition; //positon when get violationImage
+    uint64_t violationTime; ///< time when get violationImage
+    org::jeecg::defsPatrol::grpc::GPSPoint violationPosition; ///< positon when get violationImage
 
-    bool isFSM = false; //Fire and Smoke Monitor
-    int32_t fireStatus; //0 no fire 1 has fire
+    bool isFSM = false; ///< Fire and Smoke Monitor
+    int32_t fireStatus; ///< 0 no fire 1 has fire
     QByteArray fireImage;
-    uint64_t fireTime; //time when get fireImage
-    org::jeecg::defsPatrol::grpc::GPSPoint firePosition; //positon when get fireImage
+    uint64_t fireTime; ///< time when get fireImage
+    org::jeecg::defsPatrol::grpc::GPSPoint firePosition; ///< positon when get fireImage
 
-    bool isTSGM = false; //Turn Stile Gate Monitor
-    int32_t gateStatus; //0 no gate 1 gate close 2 gate open
+    bool isTSGM = false; ///< Turn Stile Gate Monitor
+    int32_t gateStatus; ///< 0 no gate 1 gate close 2 gate open
     QByteArray gateImage;
-    uint64_t gateTime; //time when get gateImage
-    org::jeecg::defsPatrol::grpc::GPSPoint gatePosition; //positon when get gateImage
+    uint64_t gateTime; ///< time when get gateImage
+    org::jeecg::defsPatrol::grpc::GPSPoint gatePosition; ///< positon when get gateImage
 
     bool isNeedTSGM = false;
     QTime timerTSGM;
-    uint8_t statusTSGM = 0; //0 ready 1 wait for result 2 has result 3 timeout but no result
+    uint8_t statusTSGM = 0; ///< 0 ready 1 wait for result 2 has result 3 timeout but no result
     org::jeecg::defsPatrol::grpc::GPSPoint gateDestination;
 
 

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -5,12 +5,14 @@
 #include <QStringList>
 #include <math.h>
 #include <iomanip>
+#include <thread>
 
 #include "modulecomm.h"
 #include "rawpic.pb.h"
 #include "chassis.pb.h"
 #include "platform_feedback.pb.h"
 #include "gpsimu.pb.h"
+#include "math/gnss_coordinate_convert.h" //for calculate totalPathLength
 
 extern std::string gstrserverip;
 extern std::string gstruploadPort;
@@ -19,6 +21,9 @@ extern std::string gstrid;
 extern std::string gstrplateNumber;
 extern std::string gvehicleType;
 
+extern double gspeedWantAvg; // m/s
+extern double gwaitTime; // s
+
 extern char stryamlpath[256];
 
 extern uint8_t gShift_Status;//3 p 4 r 5 n 6 d
@@ -366,6 +371,7 @@ void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int
     }
 
     double GPS_Speed = sqrt(xdata.ve()*xdata.ve()+xdata.vd()*xdata.vd()+xdata.vn()*xdata.vn());
+    if (GPS_Speed < 0.01)GPS_Speed = 0.0;
 
     gMutex_GPSIMU.lock();
     GPSRTKStatus = xdata.rtk_state();
@@ -467,6 +473,24 @@ void DataExchangeClient::ListenTraceMapMsg(const char * strdata,const unsigned i
         pathPoints.append(onePoint);
 //        std::cout<<pathPoints.at(i).index()<<std::endl;
     }
+//    std::cout<<"get a tracemap"<<std::endl;
+
+    totalPathLength = 0.0;
+    double localPositionX = 0.0;
+    double localPositionNextX = 0.0;
+    double localPositionY = 0.0;
+    double localPositionNextY = 0.0;
+    for(int i = 0;i < npoint - 1;i++)
+    {
+        GaussProjCal(simpletrace[i].gps_lat,simpletrace[i].gps_lng,&localPositionX,&localPositionY);
+        GaussProjCal(simpletrace[i+1].gps_lat,simpletrace[i+1].gps_lng,&localPositionNextX,&localPositionNextY);
+        double deltaX = localPositionNextX - localPositionX;
+        double deltaY = localPositionNextY - localPositionY;
+        totalPathLength += sqrt(deltaX*deltaX + deltaY*deltaY);
+    }
+
+    arrivedTime = totalPathLength/gspeedWantAvg;
+    waitTime = gwaitTime;
 
     emit path_Updated();
 }
@@ -484,8 +508,8 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     gMutex_Chassis.unlock();
 
     gMutex_PlatformFeedback.lock();
-    request.set_statusfeedback(statusFeedback);
-    request.set_modefeedback(modeFeedback);
+    request.set_statusfeedback(statusFeedback);//statusFeedback
+    request.set_modefeedback(modeFeedback);//modeFeedback
     request.set_shiftfeedback(shiftFeedback);
     request.set_steeringwheelanglefeedback(steeringWheelAngleFeedback);
     request.set_throttlefeedback(throttleFeedback);
@@ -522,8 +546,10 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_sensorstatuscamrear(sensorStatusCamRear);
     request.set_sensorstatuscamleft(sensorStatusCamLeft);
     request.set_sensorstatuscamright(sensorStatusCamRight);
-    request.set_isarrived(isArrived);
+    request.set_isarrived(isArrived);//isArrived
     request.set_platenumber(plateNumber);
+    request.set_usestatusfeedback(useStatus);
+    request.set_remainpathlength(remainPathLength);
     request.set_classfeedback(vehicleType);
 
     // Container for the data we expect from the server.
@@ -569,6 +595,9 @@ std::string DataExchangeClient::uploadPath(void)
         request.add_pathpoints();
         request.mutable_pathpoints(i)->operator =(pathPoints.at(i));
     }
+    request.set_arrivedtime(arrivedTime);
+    request.set_waittime(waitTime);
+    request.set_totalpathlength(totalPathLength);
 
     // Container for the data we expect from the server.
     Empty reply;
@@ -663,18 +692,32 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     sensorStatusCamLeft = false;
     sensorStatusCamRight = false;
 
-    isArrived = 0; //0 no destination 1 not arrived 2 arrived
+//    isArrived = 0; ///< 0 no destination 1 not arrived 2 arrived
 
     plateNumber = gstrplateNumber;
 
+//    modeFeedback = CtrlMode::CMD_REMOTE; ///< mode Feedback
+
+    useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+    if(useStatus == org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING)
+    {
+        remainPathLength = 0.0;
+        isArrived = 1;
+    }
+
+    remainPathLength -= (speed * timeInterval_ms/1000.0);
+    if(remainPathLength <= 0.0)
+    {
+        remainPathLength = 0.0;
+        if(isArrived = 1)isArrived = 2;
+    }
+
     if(std::atoi(gvehicleType.c_str()) == 0)
         vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
     if(std::atoi(gvehicleType.c_str()) == 1)
         vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::GUIDE;
     if(std::atoi(gvehicleType.c_str()) == 2)
         vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::CLEAN;
-
-//    modeFeedback = CtrlMode::CMD_REMOTE; //mode Feedback
 }
 
 void DataExchangeClient::updatePath(std::string pathID, QVector<MapPoint> points)
@@ -735,7 +778,7 @@ void DataExchangeClient::run()
         }
         else
         {
-            std::this_thread::sleep_for(std::chrono::microseconds(500));//sleep 0.5ms
+            std::this_thread::sleep_for(std::chrono::microseconds(500));///< sleep 0.5ms
         }
 
         if(abs(xTime.elapsed() - fileWriteTime) >= 2500)
@@ -751,7 +794,7 @@ void DataExchangeClient::run()
         }
         else
         {
-            std::this_thread::sleep_for(std::chrono::microseconds(500));//sleep 0.5ms
+            std::this_thread::sleep_for(std::chrono::microseconds(500));///< sleep 0.5ms
         }
     }
 }
@@ -801,6 +844,7 @@ void DataExchangeClient::destination_Recieved_Slot(void)
     if(fabs(destinationPosition.latitude()) > 0.000001 \
             && fabs(destinationPosition.longitude()) > 0.000001)
     {
+//        std::cout<<"get destination"<<std::endl;
         if(destinationRefreshed == true)
         {
             xodrobj xodrDest;
@@ -808,14 +852,16 @@ void DataExchangeClient::destination_Recieved_Slot(void)
             xodrDest.flat = destinationPosition.latitude();
             xodrDest.lane = 1;
 
-            iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); // send request msg
-            destinationRefreshed = false;
+            iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
+//            std::cout<<"modulesend xodr request"<<std::endl;
         }
     }
 }
 
 void DataExchangeClient::path_Updated_Slot(void)
 {
+    remainPathLength = totalPathLength;
+    isArrived = 1; ///< 0 no destination 1 not arrived 2 arrived
     uploadPath();
     if(destinationRefreshed == true)
         destinationRefreshed = false;

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -118,20 +118,20 @@ private:
 
     std::string id;
     uint64_t timeStamp = QDateTime::currentMSecsSinceEpoch();
-    double SOC; //0.0-100.0%
-    VehicleStatus statusFeedback = VehicleStatus::STATUS_EMERGENCY_STOP;
+    double SOC = 0.0; //0.0-100.0%
+    VehicleStatus statusFeedback = VehicleStatus::STATUS_AUTO;
     double mileage = 0.0; // kilometer
     uint64_t timeMileageRecord = QDateTime::currentMSecsSinceEpoch();
     double speed; // m/s
     org::jeecg::defsDetails::grpc::ShiftStatus shiftFeedback = org::jeecg::defsDetails::grpc::ShiftStatus::SHIFT_PARKING;
-    double steeringWheelAngleFeedback; //+/-540 degree
-    double throttleFeedback;
-    double brakeFeedback;
-    int32_t GPSRTKStatus; //GPS-RTK status 0-6 6 is best
+    double steeringWheelAngleFeedback = 0.0; //+/-540 degree
+    double throttleFeedback = 0.0;
+    double brakeFeedback = 0.0;
+    int32_t GPSRTKStatus = 0; //GPS-RTK status 0-6 6 is best
     org::jeecg::defsDetails::grpc::GPSPoint positionFeedback;
-    double pitch;
-    double roll;
-    double heading;
+    double pitch = 0.0;
+    double roll = 0.0;
+    double heading = 0.0;
 
     QMutex gMutex_Chassis;
     QMutex gMutex_GPSIMU;
@@ -146,27 +146,31 @@ private:
     QByteArray cameraImageRight;
     QMutex gMutex_ImageRight;
 
-    bool sensorStatusGPSIMU; //0 GPS-IMU ok 1 GPS-IMU error
-    bool sensorStatusLidar;
-    bool sensorStatusRadar;
-    bool sensorStatusCamFront;
-    bool sensorStatusCamRear;
-    bool sensorStatusCamLeft;
-    bool sensorStatusCamRight;
+    bool sensorStatusGPSIMU = false; ///< 0 GPS-IMU ok 1 GPS-IMU error
+    bool sensorStatusLidar = false;
+    bool sensorStatusRadar = false;
+    bool sensorStatusCamFront = false;
+    bool sensorStatusCamRear = false;
+    bool sensorStatusCamLeft = false;
+    bool sensorStatusCamRight = false;
 
-    int32_t isArrived; //0 no destination 1 not arrived 2 arrived
+    int32_t isArrived = 1; ///< 0 no destination 1 not arrived 2 arrived ,20220414 remove 0
 
-    std::string plateNumber;
+    std::string plateNumber = "津A123456";
+    org::jeecg::defsDetails::grpc::CtrlMode modeFeedback = org::jeecg::defsDetails::grpc::CtrlMode::CMD_AUTO; //mode Feedback
 
+    org::jeecg::defsDetails::grpc::UseStatus useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+    double remainPathLength = 0.0;
     org::jeecg::defsDetails::grpc::VehicleClass vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
 
-    org::jeecg::defsDetails::grpc::CtrlMode modeFeedback = org::jeecg::defsDetails::grpc::CtrlMode::CMD_EMERGENCY_STOP; //mode Feedback
-
     org::jeecg::defsDetails::grpc::GPSPoint destinationPosition;
     bool destinationRefreshed = false;
 
     std::string patrolPathID;
     QVector<org::jeecg::defsDetails::grpc::MapPoint> pathPoints;
+    double arrivedTime = 0.0; ///< 到达路径终点预计耗时 s
+    double waitTime = 0.0; ///< 到达目的地后原地待命的时间 s
+    double totalPathLength = 0.0; ///< 路径总长度 m
 
 signals:
     void uploadPath_Finished(std::string pathID);