fix(grpc_BS):fix path planning function for guide vehicle

src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -34,6 +34,17 @@ struct msgunit
 
 #endif
 
+#ifndef GLOBAL_GPS_POINT
+#define GLOBAL_GPS_POINT
+struct gGPSPoint
+{
+    double latitude = 0.0;
+    double longitude = 0.0;
+    double height = 0.0;
+};
+#endif
+QVector<gGPSPoint> gPOIPoints;
+
 uint8_t gShift_Status = 3; //3 p 4 r 5 n 6 d
 uint8_t gCtrlMode_Status = 0; //0 auto 1 remote 2 stop 3 platform
 

src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -17,6 +17,17 @@ extern char stryamlpath[256];
 
 extern uint8_t gCtrlMode_Status; //0 auto 1 remote 2 stop 3 platform
 
+#ifndef GLOBAL_GPS_POINT
+#define GLOBAL_GPS_POINT
+struct gGPSPoint
+{
+    double latitude = 0.0;
+    double longitude = 0.0;
+    double height = 0.0;
+};
+#endif
+extern QVector<gGPSPoint> gPOIPoints;
+
 using org::jeecg::defsControl::grpc::Empty; ///< other message
 using org::jeecg::defsControl::grpc::GPSPoint;
 using org::jeecg::defsControl::grpc::MapPoint;
@@ -375,6 +386,14 @@ void VehicleUploadMapClient::updateMapPOIData(void)
 {
     std::cout<<"isNeedMap:"<<isNeedMap<<std::endl;
     std::cout<<"patrolPathID:"<<patrolPathID<<std::endl;
+    gPOIPoints.clear();
+    struct gGPSPoint tempPoint;
+    for(int i=0;i<POIPoints.size();i++)
+    {
+        tempPoint.latitude = POIPoints.at(i).mappoint().latitude();
+        tempPoint.longitude = POIPoints.at(i).mappoint().longitude();
+        gPOIPoints.append(tempPoint);
+    }
     emit patrolPOI_Recieved(patrolPathID);
 }
 

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -29,6 +29,17 @@ extern char stryamlpath[256];
 extern uint8_t gShift_Status;//3 p 4 r 5 n 6 d
 extern uint8_t gCtrlMode_Status; //0 auto 1 remote 2 stop 3 platform
 
+#ifndef GLOBAL_GPS_POINT
+#define GLOBAL_GPS_POINT
+struct gGPSPoint
+{
+    double latitude = 0.0;
+    double longitude = 0.0;
+    double height = 0.0;
+};
+#endif
+extern QVector<gGPSPoint> gPOIPoints;
+
 using org::jeecg::defsDetails::grpc::Empty; ///< other message
 using org::jeecg::defsDetails::grpc::GPSPoint;
 using org::jeecg::defsDetails::grpc::MapPoint;
@@ -64,6 +75,23 @@ DataExchangeClient::DataExchangeClient(std::shared_ptr<Channel> channel)
     shmPlatformFeedback.mpa = iv::modulecomm::RegisterRecvPlus(shmPlatformFeedback.mstrmsgname,funupdate);
     funupdate = std::bind(&DataExchangeClient::ListenTraceMapMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     shmTraceMap.mpa = iv::modulecomm::RegisterRecvPlus(shmTraceMap.mstrmsgname,funupdate);
+
+    id = gstrid;
+    plateNumber = gstrplateNumber;
+    switch (std::atoi(gvehicleType.c_str())) {
+    case 0:
+        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
+        break;
+    case 1:
+        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::GUIDE;
+        break;
+    case 2:
+        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::CLEAN;
+        break;
+    default:
+        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
+        break;
+    }
 }
 
 DataExchangeClient::~DataExchangeClient(void)
@@ -75,6 +103,7 @@ DataExchangeClient::~DataExchangeClient(void)
     if(shmChassis.mpa != nullptr)iv::modulecomm::Unregister(shmChassis.mpa);
     if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.mpa);
     if(shmPlatformFeedback.mpa != nullptr)iv::modulecomm::Unregister(shmPlatformFeedback.mpa);
+    if(shmTraceMap.mpa != nullptr)iv::modulecomm::Unregister(shmTraceMap.mpa);
     requestInterruption();
     while(this->isFinished() == false);
 }
@@ -517,9 +546,6 @@ 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);
@@ -641,7 +667,6 @@ 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;
@@ -699,12 +724,6 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     sensorStatusCamLeft = false;
     sensorStatusCamRight = false;
 
-//    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)
     {
@@ -716,8 +735,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     if(remainPathLength <= 0.0)
         remainPathLength = 0.0;
 
-    if(isArrived == 1 && fabs(destinationPosition.latitude()) > 0.000001 \
-            && fabs(destinationPosition.longitude()) > 0.000001)
+    if(isArrived == 1)
     {
         double localPositionX = 0.0;
         double localPositionNextX = 0.0;
@@ -734,21 +752,6 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
         }
     }
     std::cout<<"is arrived: "<<isArrived<<std::endl;
-
-    switch (std::atoi(gvehicleType.c_str())) {
-    case 0:
-        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
-        break;
-    case 1:
-        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::GUIDE;
-        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)
@@ -832,18 +835,22 @@ void DataExchangeClient::run()
 
 void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
 {
-    if(destinationRefreshed == false)
-    {
         isArrived = 0;
-        destinationRefreshed = true;
         patrolPathID = pathID;
+
+        xodrobj xodrDest;
+        xodrDest.flon = gPOIPoints.at(1).longitude;
+        xodrDest.flat = gPOIPoints.at(1).latitude;
+        xodrDest.lane = 1;
+
+        destinationSaved.set_latitude(gPOIPoints.at(1).latitude);
+        destinationSaved.set_longitude(gPOIPoints.at(1).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;
         if(destinationRefreshedTimer.isActive() == false)destinationRefreshedTimer.start(10000);
 //        std::cout<<"patrol path calculating"<<std::endl;
-    }
-    else
-    {
-        std::cout<<"last path calculating has not finished, please wait."<<std::endl;
-    }
 
 //    QFile mapfile;
 //    mapfile.setFileName("/home/nvidia/Documents/path2.txt");
@@ -867,32 +874,12 @@ void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
 //    uploadPath();
 }
 
-void DataExchangeClient::ctrlMode_Changed_Slot(org::jeecg::defsDetails::grpc::CtrlMode ctrlMode)
-{
-    modeFeedback = ctrlMode;
-}
-
 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;
-            xodrDest.flon = destinationPosition.longitude();
-            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;
-        }
+
     }
 }
 
@@ -900,22 +887,17 @@ void DataExchangeClient::path_Updated_Slot(void)
 {
     if(destinationRefreshedTimer.isActive() == true)
         destinationRefreshedTimer.stop();
-    if(destinationRefreshed == true)
-        destinationRefreshed = false;
     remainPathLength = totalPathLength;
     isArrived = 1; ///< 0 no path 1 not arrived 2 arrived
     uploadPath();
-
 }
 
 void DataExchangeClient::destination_Refreshed_Timeout(void)
 {
-    if(destinationRefreshed == true)   
-        destinationRefreshed = false;
-    remainPathLength = 0;
+    if(destinationRefreshedTimer.isActive() == true)
+        destinationRefreshedTimer.stop();
+    remainPathLength = 0.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

@@ -165,7 +165,6 @@ private:
 
     org::jeecg::defsDetails::grpc::GPSPoint destinationPosition;
     org::jeecg::defsDetails::grpc::GPSPoint destinationSaved;
-    bool destinationRefreshed = false;
     QTimer destinationRefreshedTimer;
 
     std::string patrolPathID;
@@ -181,7 +180,6 @@ signals:
 
 public slots:
     void patrolPOI_Recieved_Slot(std::string pathID);
-    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);