add distance_pass_end

src/decition/decition_brain_sf/decition/adc_adapter/sightseeing_adapter.cpp

@@ -245,7 +245,7 @@ iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_I
     (*decition)->angSpeed=600;
 
     //日志输出
-    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f,accAim: %f,handBrake: %d,dangWei: %d,distance_to_end: %f",
+    givlog->debug("decition_brain","dSpeed: %f,realspeed: %f,brake: %f,torque: %f,accAim: %f,handBrake: %d,dangWei: %d,distance_to_end: %f",
                             dSpeed,realSpeed,(*decition)->brake,(*decition)->torque,accAim,(*decition)->handBrake,(*decition)->dangWei,distance_to_end);
 
     //垃圾车，控制输出，其它

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -199,6 +199,7 @@ static int obstacle_avoid_flag=0;
 static int front_car_id=-1;
 static int front_car_vector_id=-1;
 static bool final_brake_lock=false,brake_mode=false;
+double distance_pass_end=0;
 
 //日常展示
 
@@ -1070,9 +1071,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                         if(PathPoint+forecast_final_point>gpsMapLine.size())
                         {                           
                             distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);
+                            distance_pass_end=computeDistPassEnd(gpsMapLine,PathPoint);
                             DistanceToEnd=distance_to_end;
-                            givlog->debug("decition_brain","distoend: %f",
-                                            distance_to_end);
+//                            givlog->debug("decition_brain","distoend: %f",
+//                                            distance_to_end);
                             if((forecast_final>=distance_to_end)&&(realspeed>3)){
                                                         final_brake=true;
                                                         if(BrakePoint==-1)
@@ -1138,8 +1140,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             vehState=normalRun;
         }
     }
-    givlog->debug("decition_brain","avoidXNew: %d",
-                    avoidXNew);
+//    givlog->debug("decition_brain","avoidXNew: %d",
+//                    avoidXNew);
 #else
     if(obstacle_avoid_flag==1){
         avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
@@ -2304,8 +2306,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
 
-    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d",
-            vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2);
+    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d,distance_pass_end: %f",
+            vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2,distance_pass_end);
 
     if(ServiceCarStatus.txt_log_on==true){
         QDateTime dt = QDateTime::currentDateTime();
@@ -4229,6 +4231,15 @@ double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gp
     return dist_to_end;
 }
 
+double iv::decition::DecideGps00::computeDistPassEnd(const std::vector<GPSData> gpsMapLine,int pathpoint){
+    double dist_pass_end=0;
+    while(gpsMapLine[pathpoint]->mode2==23){
+        dist_pass_end+=GetDistance(*gpsMapLine[pathpoint], *gpsMapLine[pathpoint-1]);
+        pathpoint--;
+    }
+    return dist_pass_end;
+}
+
 void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft, int* avoidXRight ){
 
     double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth/2);

src/decition/decition_brain_sf/decition/decide_gps_00.h

@@ -240,6 +240,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);
+    double computeDistPassEnd(const std::vector<GPSData> gpsMapLine,int pathpoint);
     void   computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft,int* avoidXRight );
     int computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight);
     void computeObsOnRoadNew(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,