modify final park

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -956,18 +956,23 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     DecideGps00::lastGpsIndex = PathPoint;
 
-    double brake_distance=200;
-    brake_distance=max(250,(int)(dSpeed*dSpeed+150));
+//    double brake_distance=200;
+//    brake_distance=max(250,(int)(dSpeed*dSpeed+150));
 
 
     int nmapsize = gpsMapLine.size();
-    double distoend = sqrt(pow(now_gps_ins.gps_x - gpsMapLine[nmapsize-1]->gps_x,2)
-                           +pow(now_gps_ins.gps_y - gpsMapLine[nmapsize-1]->gps_y,2));
+
 
     double acc_end = 0.1;
 
     if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
     {
+                static double distoend=1000;
+                if(PathPoint+1000>gpsMapLine.size()){
+                    distoend=computeDistToEnd(gpsMapLine,PathPoint);
+                }else{
+                    distoend=1000;
+                }
                 if(!circleMode && distoend<50){
                         double nowspeed = realspeed/3.6;
                         if((distoend<10)||(distoend<(nowspeed*nowspeed)))
@@ -980,24 +985,26 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         if(distoend < 0.1)acc_end = -0.5;
                 }
     }else{
-                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
-                        minDecelerate=-0.5;
-                        std::cout<<"map finish brake"<<std::endl;
-                }
-
+//                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
+//                        minDecelerate=-0.5;
+//                        std::cout<<"map finish brake"<<std::endl;
+//                }
                 if(!circleMode){
-
                         double forecast_final=secSpeed*secSpeed+5;
-                        int forecast_final_point=((int)forecast_final)*10;
+                        int forecast_final_point=((int)forecast_final)*10+1500;
                         static int BrakePoint=-1;
                         static bool final_brake=false,final_brake_lock=false;
-                        if(PathPoint+forecast_final_point<gpsMapLine.size())
-                        {
-                            if(gpsMapLine[PathPoint+forecast_final_point]->mode2==23 && realspeed>3){
+                        static double distance_to_end=1000;
+                        if(PathPoint+forecast_final_point>gpsMapLine.size())
+                        {                           
+                            distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);
+                            if((forecast_final>=distance_to_end)&&(realspeed>3)){
                                                         final_brake=true;
                                                         if(BrakePoint==-1)
                                                              BrakePoint=PathPoint-10;
                             }
+                        }else{
+                            distance_to_end=1000;
                         }
                         if(PathPoint<BrakePoint)
                         {
@@ -1010,12 +1017,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                             minDecelerate=-0.5;
                                 }else{
                                             dSpeed=min(dSpeed, 3.0);
-                                            final_brake_lock=true;
-                                            if(PathPoint+50<gpsMapLine.size()){
-                                                if(gpsMapLine[PathPoint+50]->mode2==23){
+                                            final_brake_lock=true;                                            
+                                            if(distance_to_end<5){
                                                             minDecelerate=-0.5;
-                                                }
                                             }
+
                                 }
                         }
                 }
@@ -3933,3 +3939,12 @@ float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData g
     }
     return x;
 }
+
+double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint){
+    double dist_to_end=0;
+    for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
+        if(gpsMapLine[i]->mode2!=23)
+            dist_to_end+=GetDistance(*gpsMapLine[i+1], *gpsMapLine[i]);
+    }
+    return dist_to_end;
+}

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -214,6 +214,7 @@ public:
 
     void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
     float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);
+    double computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint);
 
     GPS_INS aim_gps_ins;
     GPS_INS chaoche_start_gps;