fix(grpc_BS):fix some error about remain path length.

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.cpp

@@ -209,13 +209,13 @@ void VehiclePatrolExceptionClient::ListenGPSIMUMsg(const char * strdata,const un
         return;
     }
 
-    double speed = sqrt(xdata.ve()*xdata.ve() + xdata.vn()*xdata.vn() + xdata.vd()*xdata.vd());
+//    double speed = sqrt(xdata.ve()*xdata.ve() + xdata.vn()*xdata.vn() + xdata.vd()*xdata.vd());
 
     mutex_GPSIMU.lock();
     currentPosition.set_latitude(xdata.lat());
     currentPosition.set_longitude(xdata.lon());
     currentPosition.set_height(xdata.height());
-    currentSpeed = speed;
+    currentSpeed = xdata.speed();
     mutex_GPSIMU.unlock();
 
 }

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -96,6 +96,7 @@ DataExchangeClient::DataExchangeClient(std::shared_ptr<Channel> channel)
 
 DataExchangeClient::~DataExchangeClient(void)
 {
+    if(shmXodrRequest.mpa != nullptr)iv::modulecomm::Unregister(shmXodrRequest.mpa);
     if(shmPicFront.mpa != nullptr)iv::modulecomm::Unregister(shmPicFront.mpa);
     if(shmPicRear.mpa != nullptr)iv::modulecomm::Unregister(shmPicRear.mpa);
     if(shmPicLeft.mpa != nullptr)iv::modulecomm::Unregister(shmPicLeft.mpa);
@@ -400,8 +401,8 @@ void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int
         return;
     }
 
-    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;
+//    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();
@@ -411,7 +412,9 @@ void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int
     pitch = xdata.pitch();
     roll = xdata.roll();
     heading = xdata.heading();
-    speed = GPS_Speed;
+    speed = xdata.speed();
+//    std::cout<<"speed: "<<speed<<std::endl;
+    if(speed < 0.01)speed = 0.0;
     gMutex_GPSIMU.unlock();
 //    std::cout<<heading<<std::endl;
 }
@@ -503,7 +506,7 @@ 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;
+    std::cout<<"get a tracemap"<<std::endl;
 
     totalPathLength = 0.0;
     double localPositionX = 0.0;
@@ -512,17 +515,18 @@ void DataExchangeClient::ListenTraceMapMsg(const char * strdata,const unsigned i
     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);
+        GaussProjCal(simpletrace[i].gps_lng,simpletrace[i].gps_lat,&localPositionX,&localPositionY);
+        GaussProjCal(simpletrace[i+1].gps_lng,simpletrace[i+1].gps_lat,&localPositionNextX,&localPositionNextY);
         double deltaX = localPositionNextX - localPositionX;
         double deltaY = localPositionNextY - localPositionY;
         totalPathLength += sqrt(deltaX*deltaX + deltaY*deltaY);
     }
+    std::cout<<"point number: "<<npoint<<",total length: "<<totalPathLength<<std::endl;
 
     arrivedTime = totalPathLength/gspeedWantAvg;
     waitTime = gwaitTime;
-
-    emit path_Updated();
+    if(destinationRefreshedTimer.isActive() == true)
+        emit path_Updated();
 }
 
 std::string DataExchangeClient::uploadVehicleInfo(void)
@@ -555,19 +559,20 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_roll(roll);
     request.set_heading(heading);
     gMutex_GPSIMU.unlock();
-
-    gMutex_ImageFront.lock();
-    request.set_cameraimagefront(cameraImageFront.data(),cameraImageFront.size());
-    gMutex_ImageFront.unlock();
-    gMutex_ImageRear.lock();
-    request.set_cameraimagerear(cameraImageRear.data(),cameraImageRear.size());
-    gMutex_ImageRear.unlock();
-    gMutex_ImageLeft.lock();
-    request.set_cameraimageleft(cameraImageLeft.data(),cameraImageLeft.size());
-    gMutex_ImageLeft.unlock();
-    gMutex_ImageRight.lock();
-    request.set_cameraimageright(cameraImageRight.data(),cameraImageRight.size());
-    gMutex_ImageRight.unlock();
+//  std::cout<<std::setprecision(12)<<"vehicle position: "<<positionFeedback.latitude()<<","<<positionFeedback.longitude()<<std::endl;
+
+//    gMutex_ImageFront.lock();
+//    request.set_cameraimagefront(cameraImageFront.data(),cameraImageFront.size());
+//    gMutex_ImageFront.unlock();
+//    gMutex_ImageRear.lock();
+//    request.set_cameraimagerear(cameraImageRear.data(),cameraImageRear.size());
+//    gMutex_ImageRear.unlock();
+//    gMutex_ImageLeft.lock();
+//    request.set_cameraimageleft(cameraImageLeft.data(),cameraImageLeft.size());
+//    gMutex_ImageLeft.unlock();
+//    gMutex_ImageRight.lock();
+//    request.set_cameraimageright(cameraImageRight.data(),cameraImageRight.size());
+//    gMutex_ImageRight.unlock();
 
     request.set_sensorstatusgpsimu(sensorStatusGPSIMU);
     request.set_sensorstatuslidar(sensorStatusLidar);
@@ -577,6 +582,7 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_sensorstatuscamleft(sensorStatusCamLeft);
     request.set_sensorstatuscamright(sensorStatusCamRight);
     request.set_isarrived(isArrived);
+//    std::cout<<"isarrived: "<<isArrived<<std::endl;
     request.set_platenumber(plateNumber);
     request.set_usestatusfeedback(useStatus);
     request.set_remainpathlength(remainPathLength);
@@ -673,6 +679,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     gMutex_GPSIMU.lock();
     mileage += (speed * timeInterval_ms/1000.0)/1000.0; // kilometer
     tempPosition.CopyFrom(positionFeedback);
+//    std::cout<<std::setprecision(12)<<tempPosition.latitude()<<","<<tempPosition.longitude()<<std::endl;
     gMutex_GPSIMU.unlock();
 //    speed = 0.0; // m/s
 //    shiftFeedback = ShiftStatus::SHIFT_DRIVE;
@@ -727,11 +734,13 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
     if(useStatus == org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING)
     {
+        destinationSaved.Clear();
         remainPathLength = 0.0;
         isArrived = 0;
     }
 
     remainPathLength -= (speed * timeInterval_ms/1000.0);
+    if(remainPathLength > 0.0)std::cout<<"remainLength: "<<remainPathLength<<std::endl;
     if(remainPathLength <= 0.0)
         remainPathLength = 0.0;
 
@@ -741,17 +750,22 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
         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);
+        GaussProjCal(tempPosition.longitude(),tempPosition.latitude(),&localPositionX,&localPositionY);
+        GaussProjCal(destinationSaved.longitude(),destinationSaved.latitude(),&localPositionNextX,&localPositionNextY);
         double deltaX = localPositionNextX - localPositionX;
         double deltaY = localPositionNextY - localPositionY;
         double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
-        if(distance < 1.0)
+        if(distance < 20.0)
+            std::cout<<"distance: "<<distance<<std::endl;
+        if(distance < 1.5)
         {
             isArrived = 2;
+            remainPathLength = 0.0;
+            destinationSaved.Clear();
+            std::cout<<"destination has arrived"<<std::endl;
         }
     }
-    std::cout<<"is arrived: "<<isArrived<<std::endl;
+//    std::cout<<"is arrived: "<<isArrived<<std::endl;
 }
 
 void DataExchangeClient::updatePath(std::string pathID, QVector<MapPoint> points)
@@ -807,7 +821,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
@@ -836,21 +850,20 @@ void DataExchangeClient::run()
 void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
 {
         isArrived = 0;
+        remainPathLength = 0.0;
+        destinationSaved.Clear();
         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);
+        std::cout<<"destination: "<<std::setprecision(12)<<gPOIPoints.at(1).longitude<<","<<gPOIPoints.at(1).latitude<<std::endl;
 
         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;
+        std::cout<<"patrol path calculating"<<std::endl;
 
 //    QFile mapfile;
 //    mapfile.setFileName("/home/nvidia/Documents/path2.txt");
@@ -887,6 +900,9 @@ void DataExchangeClient::path_Updated_Slot(void)
 {
     if(destinationRefreshedTimer.isActive() == true)
         destinationRefreshedTimer.stop();
+    destinationSaved.set_latitude(pathPoints.at(pathPoints.size()-1).mappoint().latitude());
+    destinationSaved.set_longitude(pathPoints.at(pathPoints.size()-1).mappoint().longitude());
+    destinationSaved.set_height(0.0);
     remainPathLength = totalPathLength;
     isArrived = 1; ///< 0 no path 1 not arrived 2 arrived
     uploadPath();
@@ -896,6 +912,7 @@ void DataExchangeClient::destination_Refreshed_Timeout(void)
 {
     if(destinationRefreshedTimer.isActive() == true)
         destinationRefreshedTimer.stop();
+    destinationSaved.Clear();
     remainPathLength = 0.0;
     isArrived = 0;
     emit uploadPath_Finished("");