添加晓韦的逻辑

src/decition/decition_brain_sf_jsrunlegs_new/decition/decide_gps_00.cpp

@@ -1581,13 +1581,15 @@ static int file_num;
         dSpeed = min(25.0,dSpeed);
 
     }
-#ifdef AVOID_NEW  //20220223toggle
-//    if((vehState==avoiding)||(vehState==preAvoid))
-//    {
-//        dSpeed = min(8.0,dSpeed);
-//    }else if((vehState==backOri)||(avoidXNew!=0)){
-//        dSpeed = min(12.0,dSpeed);
-//    }
+#ifdef AVOID_NEW
+    if((vehState==avoiding)||(vehState==preAvoid))
+    {
+        //dSpeed = min(8.0,dSpeed);
+        dSpeed = min(4.0,dSpeed);
+    }else if((vehState==backOri)||(avoidXNew!=0)){
+        //dSpeed = min(12.0,dSpeed);
+        dSpeed = min(5.0,dSpeed);
+    }
 #else
     if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
     {
@@ -1600,9 +1602,9 @@ static int file_num;
         dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
     }
     //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
-    if(brake_mode==true){
-        dSpeed=min(dSpeed, 3.0);
-    }
+//    if(brake_mode==true){
+//        dSpeed=min(dSpeed, 3.0);
+//    }
 
     if(front_car_id>0){
         static bool brake_state=false;
@@ -1640,7 +1642,7 @@ static int file_num;
     static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
     if(!ServiceCarStatus.daocheMode){
         //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        computeObsOnRoadXY(lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
 
 
         if(obs_filter_flag==0)
@@ -1755,7 +1757,7 @@ static int file_num;
 //            gps_decition->rightlamp = false;
 //        }
 
-        dSpeed = min(5.0,dSpeed);
+        //dSpeed = min(6.0,dSpeed);
         if (turnLampFlag>0)
         {
             gps_decition->leftlamp = false;
@@ -1961,7 +1963,7 @@ static int file_num;
     if (vehState == avoidObs   || vehState==waitAvoid ) {
         if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
         {
-            dSpeed = min(5.0,dSpeed);
+            dSpeed = min(4.0,dSpeed);
             avoidTimes++;
             //          if (avoidTimes>=6)
             if (avoidTimes>=ServiceCarStatus.aocfTimes)
@@ -3741,13 +3743,17 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
 
 }
 
-void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
                                                  const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
 
     //    QTime xTime;
     //   xTime.start();
     //    qDebug("time 0 is %d ",xTime.elapsed());
     double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
 
     if (lidarGridPtr == NULL)
     {
@@ -3762,7 +3768,15 @@ void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr
         if(!ServiceCarStatus.useMobileEye){
             lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
         }
-        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+
+        obsCarCoordinateX=obsPoint.x;
+        obsCarCoordinateY=obsPoint.y;
+        obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+        obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+        givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
         //    lidarDistance=-1;
         if (lidarDistance<0)
         {

src/decition/decition_brain_sf_jsrunlegs_new/decition/decide_gps_00.h

@@ -211,7 +211,7 @@ public:
     bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
 
     void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
-    void computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    void computeObsOnRoadXY(iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
     static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
 
     void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);