decition_brain_sf_changan_sf.

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.cpp

@@ -112,6 +112,14 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
 
     int nsize = trace.size();
 
+    if(nsize == 0)
+    {
+        std::cout<<" in pid controller. no trace , angle is 0."<<std::endl;
+        return 0;
+    }
+
+    assert(nsize != 0);
+
     for (int i = 1; i < nsize-1; i++)
     {
         sumdis += GetDistance(trace[i - 1], trace[i]);

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -186,6 +186,8 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
     ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
 
+    std::string strvehtype = ServiceCarStatus.msysparam.mvehtype;
+
     ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
     ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
 

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -1277,11 +1277,17 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
                 getGpsTraceNowLeftRight(gpsTraceNow);
             }
-            else
+            else //if((vehState==preAvoid)||(vehState==normalRun)||(vehState))
             {
-                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
-                getGpsTraceNowLeftRight(gpsTraceNow);
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
             }
+//            else
+//            {
+//                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
+//                getGpsTraceNowLeftRight(gpsTraceNow);
+//            }
         }
     }
 #else
@@ -1624,7 +1630,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else
         {
-            vehState==normalRun;
+            vehState=normalRun;
         }
     }
 
@@ -1814,6 +1820,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
             vehState=normalRun;
             obstacle_avoid_flag=0;
+            avoidXNewPreVector.clear();
         }
     }
 
@@ -2005,30 +2012,109 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         double* avoidRight_p=&avoidRight_value;
         computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
         avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
-        if(avoidXNewPreVector.size()<5){
+
+        givlog->debug("decition_brain","======avoidXNewPre==========:%f",avoidXNewPre);
+
+        const int nAvoidPreCount = 100;
+
+        if(avoidXNewPreVector.size()<nAvoidPreCount){
             avoidXNewPreVector.push_back(avoidXNewPre);
-        }else{
-            if(avoidXNewPreVector[0]!=avoidXNewLast){
-                for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
-                    if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
-                        avoidXNewPreFilter=avoidXNewLast;
-                        break;
+        }
+        else
+        {
+            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
+            avoidXNewPreVector.push_back(avoidXNewPre);
+        }
+
+        if(vehState == preAvoid)
+        {
+            avoidXNew = avoidXNewPre;
+        }
+        else
+        {
+            if(fabs(avoidXNewLast)>0.1)
+            {
+
+                bool bOriginSafe = true;
+
+                if(avoidXNewPreVector.size()<nAvoidPreCount)
+                {
+                    bOriginSafe = false;
+                }
+                else
+                {
+                    unsigned int j;
+                    unsigned int nSize = avoidXNewPreVector.size();
+                    for(j=0;j<nSize;j++)
+                    {
+                        if(fabs(avoidXNewPreVector[j])>0.1)
+                        {
+                            bOriginSafe = false;
+                            break;
+                        }
+                    }
+                }
+                if(bOriginSafe)
+                {
+                    avoidXNew = 0;
+ //                   avoidXNewPreVector.clear();
+                }
+                else
+                {
+                    unsigned int j;
+                    unsigned int nSize = avoidXNewPreVector.size();
+                    if(avoidXNewPreVector.size()==nAvoidPreCount)
+                    {
+                        double filter = 0;
+                        for(j=0;j<nSize;j++)
+                        {
+                            if(fabs(avoidXNewPreVector[j])>0.1)
+                            {
+                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
+                                else
+                                {
+                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
+                                    {
+                                        filter = 0;
+                                        break;
+                                    }
+                                }
+                            }
+                        }
+                        if(fabs(filter)<0.1)
+                        {
+                            avoidXNew = avoidXNewLast;
+                        }
+                        else
+                        {
+                            if(fabs(filter - avoidXNewLast)>=2.0)
+                            {
+                                avoidXNew = filter;
+                            }
+                            else
+                            {
+                                avoidXNew = avoidXNewLast;
+                            }
+                        }
+                    }
+                    else
+                    {
+                        avoidXNew = avoidXNewLast;
                     }
-                    if(i==avoidXNewPreVector.size()-1)
-                        avoidXNewPreFilter=avoidXNewPreVector[0];
                 }
             }
-            avoidXNewPreVector.clear();
         }
-        if(avoidXNewPreFilter!=avoidXNewLast){
-            avoidXNew=avoidXNewPre;
-            if(avoidXNew<0)
-                turnLampFlag=-1;
-            else if(avoidXNew>0)
-                turnLampFlag=1;
-            else
-                turnLampFlag=0;
 
+
+        if(avoidXNew<0)
+            turnLampFlag=-1;
+        else if(avoidXNew>0)
+            turnLampFlag=1;
+        else
+            turnLampFlag=0;
+
+        if(fabs(avoidXNew - avoidXNewLast)>0.1)
+        {
             gpsTraceNow.clear();
             gpsTraceAvoidXY.clear();
             givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
@@ -2040,8 +2126,45 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             obstacle_avoid_flag=1;
             hasCheckedAvoidLidar = false;
             avoidXNewLast=avoidXNew;
-            avoidXNewPreFilter=avoidXNew;
         }
+
+//        if(avoidXNewPreVector.size()<5){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }else{
+//            if(avoidXNewPreVector[0]!=avoidXNewLast){
+//                for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+//                    if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+//                        avoidXNewPreFilter=avoidXNewLast;
+//                        break;
+//                    }
+//                    if(i==avoidXNewPreVector.size()-1)
+//                        avoidXNewPreFilter=avoidXNewPreVector[0];
+//                }
+//            }
+//            avoidXNewPreVector.clear();
+//        }
+//        if(avoidXNewPreFilter!=avoidXNewLast){
+//            avoidXNew=avoidXNewPre;
+//            if(avoidXNew<0)
+//                turnLampFlag=-1;
+//            else if(avoidXNew>0)
+//                turnLampFlag=1;
+//            else
+//                turnLampFlag=0;
+
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//            avoidXNewPreFilter=avoidXNew;
+//        }
     }
 //20230407,
 //    if((vehState == preAvoid)||((avoidXNew!=0)))
@@ -3777,6 +3900,7 @@ void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::
     }
     else {
         //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+
         obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
         float lidarXiuZheng=0;
         if(!ServiceCarStatus.useMobileEye){