fix(grpc_BS):add guide vehicle logic.but not finished.

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -44,6 +44,7 @@ DataExchangeClient::DataExchangeClient(std::shared_ptr<Channel> channel)
 
     QObject::connect(this,&DataExchangeClient::destination_Recieved,this,&DataExchangeClient::destination_Recieved_Slot);
     QObject::connect(this,&DataExchangeClient::path_Updated,this,&DataExchangeClient::path_Updated_Slot);
+    QObject::connect(&destinationRefreshedTimer,&QTimer::timeout,this,&DataExchangeClient::destination_Refreshed_Timeout);
 
     shmXodrRequest.mpa = iv::modulecomm::RegisterSend(shmXodrRequest.mstrmsgname,shmXodrRequest.mnBufferSize,shmXodrRequest.mnBufferCount);
 
@@ -516,6 +517,9 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_brakefeedback(brakeFeedback);
     gMutex_PlatformFeedback.unlock();
 
+    std::cout<<"mode feedback"<<modeFeedback<<std::endl;
+    std::cout<<"status feedback"<<statusFeedback<<std::endl;
+
     gMutex_GPSIMU.lock();
     request.set_mileage(mileage);
     request.set_speed(speed);
@@ -546,7 +550,7 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_sensorstatuscamrear(sensorStatusCamRear);
     request.set_sensorstatuscamleft(sensorStatusCamLeft);
     request.set_sensorstatuscamright(sensorStatusCamRight);
-    request.set_isarrived(isArrived);//isArrived
+    request.set_isarrived(isArrived);
     request.set_platenumber(plateNumber);
     request.set_usestatusfeedback(useStatus);
     request.set_remainpathlength(remainPathLength);
@@ -635,12 +639,15 @@ std::string DataExchangeClient::uploadPath(void)
 
 void DataExchangeClient::updateData(uint64_t timeInterval_ms)
 {
+    org::jeecg::defsDetails::grpc::GPSPoint tempPosition;
+
     id = gstrid;
     timeStamp = QDateTime::currentMSecsSinceEpoch();
 //    SOC = 87.5;
 //    statusFeedback = VehicleStatus::STATUS_REMOTE;
     gMutex_GPSIMU.lock();
     mileage += (speed * timeInterval_ms/1000.0)/1000.0; // kilometer
+    tempPosition.CopyFrom(positionFeedback);
     gMutex_GPSIMU.unlock();
 //    speed = 0.0; // m/s
 //    shiftFeedback = ShiftStatus::SHIFT_DRIVE;
@@ -702,22 +709,46 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     if(useStatus == org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING)
     {
         remainPathLength = 0.0;
-        isArrived = 1;
+        isArrived = 0;
     }
 
     remainPathLength -= (speed * timeInterval_ms/1000.0);
     if(remainPathLength <= 0.0)
-    {
         remainPathLength = 0.0;
-        if(isArrived = 1)isArrived = 2;
+
+    if(isArrived == 1 && fabs(destinationPosition.latitude()) > 0.000001 \
+            && fabs(destinationPosition.longitude()) > 0.000001)
+    {
+        double localPositionX = 0.0;
+        double localPositionNextX = 0.0;
+        double localPositionY = 0.0;
+        double localPositionNextY = 0.0;
+        GaussProjCal(tempPosition.latitude(),tempPosition.longitude(),&localPositionX,&localPositionY);
+        GaussProjCal(destinationSaved.latitude(),destinationSaved.longitude(),&localPositionNextX,&localPositionNextY);
+        double deltaX = localPositionNextX - localPositionX;
+        double deltaY = localPositionNextY - localPositionY;
+        double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
+        if(distance < 1.0)
+        {
+            isArrived = 2;
+        }
     }
+    std::cout<<"is arrived: "<<isArrived<<std::endl;
 
-    if(std::atoi(gvehicleType.c_str()) == 0)
+    switch (std::atoi(gvehicleType.c_str())) {
+    case 0:
         vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
-    if(std::atoi(gvehicleType.c_str()) == 1)
+        break;
+    case 1:
         vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::GUIDE;
-    if(std::atoi(gvehicleType.c_str()) == 2)
+        break;
+    case 2:
         vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::CLEAN;
+        break;
+    default:
+        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
+        break;
+    }
 }
 
 void DataExchangeClient::updatePath(std::string pathID, QVector<MapPoint> points)
@@ -773,7 +804,7 @@ void DataExchangeClient::run()
             updateData(realInterval);
             std::string reply = uploadVehicleInfo();
 //            std::cout<< reply <<std::endl;
-//            std::cout<<std::setprecision(12)<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
+            std::cout<<std::setprecision(12)<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
             lastTime = xTime.elapsed();
         }
         else
@@ -803,9 +834,11 @@ void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
 {
     if(destinationRefreshed == false)
     {
+        isArrived = 0;
         destinationRefreshed = true;
         patrolPathID = pathID;
-        std::cout<<"patrol path calculating"<<std::endl;
+        if(destinationRefreshedTimer.isActive() == false)destinationRefreshedTimer.start(10000);
+//        std::cout<<"patrol path calculating"<<std::endl;
     }
     else
     {
@@ -852,17 +885,37 @@ void DataExchangeClient::destination_Recieved_Slot(void)
             xodrDest.flat = destinationPosition.latitude();
             xodrDest.lane = 1;
 
+            destinationSaved.set_latitude(destinationPosition.latitude());
+            destinationSaved.set_longitude(destinationPosition.longitude());
+            destinationSaved.set_height(0.0);
+
             iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
 //            std::cout<<"modulesend xodr request"<<std::endl;
+            destinationRefreshed = false;
         }
     }
 }
 
 void DataExchangeClient::path_Updated_Slot(void)
 {
+    if(destinationRefreshedTimer.isActive() == true)
+        destinationRefreshedTimer.stop();
+    if(destinationRefreshed == true)
+        destinationRefreshed = false;
     remainPathLength = totalPathLength;
-    isArrived = 1; ///< 0 no destination 1 not arrived 2 arrived
+    isArrived = 1; ///< 0 no path 1 not arrived 2 arrived
     uploadPath();
-    if(destinationRefreshed == true)
+
+}
+
+void DataExchangeClient::destination_Refreshed_Timeout(void)
+{
+    if(destinationRefreshed == true)   
         destinationRefreshed = false;
+    remainPathLength = 0;
+    isArrived = 0;
+    emit uploadPath_Finished("");
+    std::cout<<"path plan timeout."<<std::endl;
+    if(destinationRefreshedTimer.isActive() == true)
+        destinationRefreshedTimer.stop();
 }

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -122,7 +122,7 @@ private:
     VehicleStatus statusFeedback = VehicleStatus::STATUS_AUTO;
     double mileage = 0.0; // kilometer
     uint64_t timeMileageRecord = QDateTime::currentMSecsSinceEpoch();
-    double speed; // m/s
+    double speed = 0.0; // m/s
     org::jeecg::defsDetails::grpc::ShiftStatus shiftFeedback = org::jeecg::defsDetails::grpc::ShiftStatus::SHIFT_PARKING;
     double steeringWheelAngleFeedback = 0.0; //+/-540 degree
     double throttleFeedback = 0.0;
@@ -154,7 +154,7 @@ private:
     bool sensorStatusCamLeft = false;
     bool sensorStatusCamRight = false;
 
-    int32_t isArrived = 1; ///< 0 no destination 1 not arrived 2 arrived ,20220414 remove 0
+    int32_t isArrived = 0; ///< 0 no path 1 not arrived 2 arrived
 
     std::string plateNumber = "津A123456";
     org::jeecg::defsDetails::grpc::CtrlMode modeFeedback = org::jeecg::defsDetails::grpc::CtrlMode::CMD_AUTO; //mode Feedback
@@ -164,7 +164,9 @@ private:
     org::jeecg::defsDetails::grpc::VehicleClass vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
 
     org::jeecg::defsDetails::grpc::GPSPoint destinationPosition;
+    org::jeecg::defsDetails::grpc::GPSPoint destinationSaved;
     bool destinationRefreshed = false;
+    QTimer destinationRefreshedTimer;
 
     std::string patrolPathID;
     QVector<org::jeecg::defsDetails::grpc::MapPoint> pathPoints;
@@ -182,6 +184,7 @@ public slots:
     void ctrlMode_Changed_Slot(org::jeecg::defsDetails::grpc::CtrlMode ctrlMode);
     void destination_Recieved_Slot(void);
     void path_Updated_Slot(void);
+    void destination_Refreshed_Timeout(void);
 };
 
 #endif // VEHICLE_UPLOAD_H