fix(grpc_BS):add pathTotalLength calculating function. and some control parameter.

src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -177,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();

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -261,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 {

src/driver/driver_cloud_grpc_client_BS/vehicle_control.h

@@ -96,6 +96,10 @@ 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::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.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

@@ -12,6 +12,7 @@
 #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;
@@ -472,6 +473,23 @@ void DataExchangeClient::ListenTraceMapMsg(const char * strdata,const unsigned i
 //        std::cout<<pathPoints.at(i).index()<<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();
 }
 
@@ -528,8 +546,8 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_sensorstatuscamright(sensorStatusCamRight);
     request.set_isarrived(isArrived);
     request.set_platenumber(plateNumber);
-    request.set_usestatusfeedback(org::jeecg::defsDetails::grpc::UseStatus::ENABLING);
-    request.set_remainpathlength(500.0);
+    request.set_usestatusfeedback(useStatus);
+    request.set_remainpathlength(remainPathLength);
     request.set_classfeedback(vehicleType);
 
     // Container for the data we expect from the server.
@@ -575,9 +593,9 @@ std::string DataExchangeClient::uploadPath(void)
         request.add_pathpoints();
         request.mutable_pathpoints(i)->operator =(pathPoints.at(i));
     }
-    request.set_arrivedtime(500.0/gspeedWantAvg);
-    request.set_waittime(gwaitTime);
-    request.set_totalpathlength(500.0);
+    request.set_arrivedtime(arrivedTime);
+    request.set_waittime(waitTime);
+    request.set_totalpathlength(totalPathLength);
 
     // Container for the data we expect from the server.
     Empty reply;
@@ -672,18 +690,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 = 0;
+    }
+
+    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)
@@ -744,7 +776,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)
@@ -760,7 +792,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
         }
     }
 }
@@ -817,7 +849,7 @@ 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
+            iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
             destinationRefreshed = false;
         }
     }
@@ -825,6 +857,8 @@ void DataExchangeClient::destination_Recieved_Slot(void)
 
 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

@@ -146,7 +146,7 @@ private:
     QByteArray cameraImageRight;
     QMutex gMutex_ImageRight;
 
-    bool sensorStatusGPSIMU; //0 GPS-IMU ok 1 GPS-IMU error
+    bool sensorStatusGPSIMU; ///< 0 GPS-IMU ok 1 GPS-IMU error
     bool sensorStatusLidar;
     bool sensorStatusRadar;
     bool sensorStatusCamFront;
@@ -154,20 +154,23 @@ private:
     bool sensorStatusCamLeft;
     bool sensorStatusCamRight;
 
-    int32_t isArrived; //0 no destination 1 not arrived 2 arrived
+    int32_t isArrived = 0; ///< 0 no destination 1 not arrived 2 arrived
 
     std::string plateNumber;
+    org::jeecg::defsDetails::grpc::CtrlMode modeFeedback = org::jeecg::defsDetails::grpc::CtrlMode::CMD_EMERGENCY_STOP; //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);