fix(grpc_BS,map_xodr):change simpletrace step lengthd, fix fsm jump signal and destination update logic.

src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

@@ -207,23 +207,27 @@ void FSMUnit::navagationSwitchChanged_Slot(bool status)
 
 void FSMUnit::useStatusCMDChanged_Slot(bool status)
 {
-    if(status)
+    if(status == true && FSMState == 3)
         workCMD = iv::brain::WorkCMD::WORK_START;
-    else
+    else if(status == false && (FSMState == 0 || FSMState == 1 || FSMState == 4))
         workCMD = iv::brain::WorkCMD::WORK_STOP;
+    else
+        workCMD = iv::brain::WorkCMD::WORK_RESERVED;
 }
 
 void FSMUnit::remoteCtrlModeChanged_Slot(bool status)
 {
-    if(status)
+    if(status == true && FSMState != 2)
         remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_ENTER;
-    else
+    else if(status == false && FSMState == 2)
         remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_EXIT;
+    else
+        remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
 }
 
 void FSMUnit::setFSMDestination_Slot(double latitude,double longitude)
 {
-    if(FSMState == 0)
+    if(FSMState == 0 || FSMState == 1 || FSMState == 4 || FSMState == 5)
     {
         currentDestination.latitude = latitude;
         currentDestination.longitude = longitude;
@@ -237,7 +241,8 @@ void FSMUnit::setFSMDestination_Slot(double latitude,double longitude)
 
 void FSMUnit::setMissionFinished_Slot(void)
 {
-    missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
+    if(FSMState == 1)
+        missionCMD = iv::brain::MissionCMD::MISSIOH_FINISH;
     currentDestination.latitude = 0.0;
     currentDestination.longitude = 0.0;
 }
@@ -345,46 +350,54 @@ void FSMUnit::arriveCheckLoop_Timeout(void)
     if(FSMState == 4) ///< 4返程
     {
 //        std::cout<<"status is return wait station"<<std::endl;
-        if(currentDestination.latitude != waitingStation.latitude || currentDestination.longitude != waitingStation.longitude)
-        {
-            currentDestination.latitude = waitingStation.latitude;
-            currentDestination.longitude = waitingStation.longitude;
-            emit sendPathPlanRequest(currentDestination.latitude,currentDestination.longitude);
-        }
         double localPositionX = 0.0;
         double localPositionNextX = 0.0;
         double localPositionY = 0.0;
         double localPositionNextY = 0.0;
-        GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
+        GaussProjCal(waitingStation.longitude,waitingStation.latitude,&localPositionX,&localPositionY);
         GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
         double deltaX = localPositionNextX - localPositionX;
         double deltaY = localPositionNextY - localPositionY;
         double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
 
-        if(distance < 1.5 && currentSpeed < 0.1)
+        if(distance > 1.0)
+        {
+            emit sendPathPlanRequest(waitingStation.latitude,waitingStation.longitude);
+        }
+        GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
+        GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
+        deltaX = localPositionNextX - localPositionX;
+        deltaY = localPositionNextY - localPositionY;
+        distance = sqrt(deltaX*deltaX + deltaY*deltaY);
+
+        if(distance < 2.0 && currentSpeed < 0.1)
         {
             waitingStationArrived = true;
         }
     }
     if(FSMState == 5) ///< 5返库
     {
-        if(currentDestination.latitude != maintainStation.latitude || currentDestination.longitude != maintainStation.longitude)
-        {
-            currentDestination.latitude = maintainStation.latitude;
-            currentDestination.longitude = maintainStation.longitude;
-            emit sendPathPlanRequest(currentDestination.latitude,currentDestination.longitude);
-        }
         double localPositionX = 0.0;
         double localPositionNextX = 0.0;
         double localPositionY = 0.0;
         double localPositionNextY = 0.0;
-        GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
+        GaussProjCal(maintainStation.longitude,maintainStation.latitude,&localPositionX,&localPositionY);
         GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
         double deltaX = localPositionNextX - localPositionX;
         double deltaY = localPositionNextY - localPositionY;
         double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
 
-        if(distance < 1.5 && currentSpeed < 0.1)
+        if(distance > 1.0)
+        {
+            emit sendPathPlanRequest(maintainStation.latitude,maintainStation.longitude);
+        }
+        GaussProjCal(currentPosition.longitude,currentPosition.latitude,&localPositionX,&localPositionY);
+        GaussProjCal(currentDestination.longitude,currentDestination.latitude,&localPositionNextX,&localPositionNextY);
+        deltaX = localPositionNextX - localPositionX;
+        deltaY = localPositionNextY - localPositionY;
+        distance = sqrt(deltaX*deltaX + deltaY*deltaY);
+
+        if(distance < 2.0 && currentSpeed < 0.1)
         {
             maintainStationArrived = true;
         }

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -507,6 +507,7 @@ void DataExchangeClient::ListenTraceMapMsg(const char * strdata,const unsigned i
         pathPoints.append(onePoint);
 //        std::cout<<pathPoints.at(i).index()<<std::endl;
     }
+    emit setFSMDestination(pathPoints.at(pathPoints.size()-1).mappoint().latitude(),pathPoints.at(pathPoints.size()-1).mappoint().longitude());
     std::cout<<"get a tracemap"<<std::endl;
 
     totalPathLength = 0.0;
@@ -913,7 +914,6 @@ void DataExchangeClient::path_Updated_Slot(void)
     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);
-    emit setFSMDestination(destinationSaved.latitude(),destinationSaved.latitude());
     remainPathLength = totalPathLength;
     isArrived = 1; ///< 0 no path 1 not arrived 2 arrived
     uploadPath();
@@ -924,7 +924,7 @@ void DataExchangeClient::destination_Refreshed_Timeout(void)
     if(destinationRefreshedTimer.isActive() == true)
         destinationRefreshedTimer.stop();
     destinationSaved.Clear();
-    emit setFSMDestination(destinationSaved.latitude(),destinationSaved.latitude());
+    emit setFSMDestination(destinationSaved.latitude(),destinationSaved.longitude());
     remainPathLength = 0.0;
     isArrived = 0;
     emit uploadPath_Finished("");

src/driver/driver_map_xodrload/main.cpp

@@ -353,7 +353,7 @@ void ShareMap(std::vector<iv::GPSData> navigation_data)
 
 
     int nsize = 100;
-    int nstep = 1;
+    float nstep = 1;
     if(navigation_data.size() < 100)
     {
         nsize = navigation_data.size();
@@ -367,7 +367,7 @@ void ShareMap(std::vector<iv::GPSData> navigation_data)
     iv::simpletrace psim[100];
     for(i=0;i<nsize;i++)
     {
-        x = *(navigation_data.at(i*nstep));
+        x = *(navigation_data.at((int)(i*nstep)));
         psim[i].gps_lat = x.gps_lat;
         psim[i].gps_lng = x.gps_lng;
         psim[i].gps_z = x.gps_z;