change decition_brain_sf_changan_shenlan. change pid controller for low speed at line road.

src/decition/common/common/car_status.h

@@ -269,6 +269,10 @@ namespace iv {
         double ang_debug=0.0;
         double torque_or_acc=0.0;
         double brake=0.0;
+
+
+        double mfbasepitch = 0.0;
+        bool mbUseRampAssit = false;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/changan_shenlan_adapter.cpp

@@ -22,6 +22,7 @@ iv::decition::ChanganShenlanAdapter::~ChanganShenlanAdapter(){
 iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                               float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
 
+    bool bUseDynamics = false;
 
     float controlSpeed=0;
     float controlBrake=0;
@@ -53,11 +54,34 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
     }
     else
     {
+
+
         controlBrake = 0;
-        if(lastTorque==0){
-            controlSpeed=100;
-        }else{
-            controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+        if(bUseDynamics == false)
+        {
+            if(lastTorque==0){
+                controlSpeed=100;
+            }else{
+                controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+            }
+        }
+        else
+        {
+            double fVehWeight = 1800;
+            double fg = 9.8;
+            double fRollForce = 50;
+            const double fRatio = 2.5;
+            double fNeed = fRollForce + fVehWeight*accAim;
+            if(ServiceCarStatus.mbUseRampAssit)
+            {
+                if(now_gps_ins.ins_pitch_angle>(ServiceCarStatus.mfbasepitch + 1.0))
+                {
+                    double fpitch = now_gps_ins.ins_heading_angle - ServiceCarStatus.mfbasepitch;
+                    double fpitchforce = fVehWeight* fg * sin(fpitch*M_PI/180.0);
+                    fNeed = fNeed + fpitchforce;
+                }
+            }
+            controlSpeed = fNeed/fRatio;
         }
     }
 
@@ -183,6 +207,10 @@ float iv::decition::ChanganShenlanAdapter::limitSpeed(float realSpeed, float con
         controlSpeed=0;
     }
 
+    controlSpeed=min((float)3000,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+
     if(realSpeed<10){
         if(realSpeed<5)
         {

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

@@ -71,6 +71,31 @@ double iv::decition::PIDController::CalcKappa(std::vector<Point2D> trace,int pre
 
 }
 
+double iv::decition::PIDController::ExtendPreviewDistance(std::vector<Point2D> & trace,double & PreviewDistance)
+{
+    double fAllowableError = 0.3;
+    int i = 0;
+    int nsize = static_cast<int>(trace.size());
+    double fPreviewAllow = 0;
+    for(i=0;i<(nsize-1);i++)
+    {
+        if(fabs(trace[i].x)>fAllowableError)
+        {
+            break;
+        }
+    }
+    if((i>=0)&&(i<nsize))
+    {
+        fPreviewAllow = fabs(trace[i].y);
+    }
+    if(fPreviewAllow > PreviewDistance)
+    {
+        PreviewDistance = fPreviewAllow;
+    }
+    return fPreviewAllow;
+
+}
+
 float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
     double ang = 0;
     double EPos = 0, EAng = 0;
@@ -94,6 +119,12 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
         realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
     }
 
+    bool bExtendPreview = false;
+    if(bExtendPreview)
+    {
+        ExtendPreviewDistance(trace,PreviewDistance);
+    }
+
 
     //    if(ServiceCarStatus.msysparam.mvehtype=="bus"){
     //        KEang = 14, KEPos = 7, DEang = 0, DEPos = 0,IEpos=0,IEang=0;

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.h

@@ -59,6 +59,8 @@ namespace iv {
 
                     private:
 
+                        double ExtendPreviewDistance(std::vector<Point2D> & trace,double & PreviewDistance);
+
                     };
 
         }

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.cpp

@@ -643,90 +643,89 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
         }
     else
     {
-                for(int i=0;i<MapTrace.size();i++)
+        for(int i=0;i<MapTrace.size();i++)
+        {
+            if(MapTrace[i]->roadMode==0){
+                doubledata[i][4]=straightSpeed;
+                if(MapTrace[i]->speed>0)
                 {
-                    if(MapTrace[i]->roadMode==0){
-                        doubledata[i][4]=straightSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==5){
-                        doubledata[i][4]=straightCurveSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==18){
-                        mode0to5countSum=mode0to5count;
-                        doubledata[i][4]=Curve_SmallSpeed;
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
 
-                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
-                        int brake_distance=(int)brake_distance_double;
-                        int step_count=0;
-                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
-                        {
-                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+                }
 
-                            for(int j=i;j>i-brake_distance;j--){
-                                doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
-                                step_count++;
-                            }
-                        }else if(mode0to5countSum>0){
-                            for(int j=i;j>=i-mode0to5countSum;j--){
-                                doubledata[j][4]=Curve_SmallSpeed;
-                                step_count++;
-                            }
-                        }else{
-                            doubledata[i][4]=Curve_SmallSpeed;
-                        }
-                        mode0to5count=0;
-                        //mode0to18count++;
-                        mode18count++;
-                        mode18flash=mode18count;
-                    }else if(MapTrace[i]->roadMode==14){
-                        mode0to18countSum=mode0to5flash+mode18flash;
-                        mode18countSum=mode18count;
-                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
-                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
-                        int brake_distanceLarge=(int)brake_distanceLarge_double;
-                        int brake_distance0to18=(int)brake_distance0to18_double;
-                        int step_count=0;
-                        doubledata[i][4]=Curve_LargeSpeed;
 
-                        if(mode18countSum>brake_distanceLarge)
-                        {
-                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
+                    }
+                }
 
-                                for(int j=i;j>i-brake_distanceLarge;j--){
-                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
-                                    step_count++;
-                                }
+            }else if(MapTrace[i]->roadMode==5){
+                doubledata[i][4]=straightCurveSpeed;
+                if(MapTrace[i]->speed>0)
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
 
-                        }else if(mode0to18countSum>brake_distance0to18){
+                }
 
-                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
 
-                                for(int j=i;j>i-brake_distance0to18;j--){
-                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
-                                    step_count++;
-                                }
-                        }else if(mode0to18countSum>0){
-                            for(int j=i;j>=i-mode0to18countSum;j--){
-                                doubledata[j][4]=Curve_LargeSpeed;
-                                step_count++;
-                            }
-                        }else{
-                                doubledata[i][4]=Curve_LargeSpeed;
-                        }
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
+                    }
+                }
 
-                        mode0to5count=0;
-                        mode18count=0;
-                        mode0to5flash=0;
-                        mode18flash=0;
+            }else if(MapTrace[i]->roadMode==18){
+
+                if(fabs(MapTrace[i]->mfCurvature)>0.06)
+                {
+                    doubledata[i][4]=(Curve_SmallSpeed + Curve_LargeSpeed)/2.0;
+                }
+                else
+                    doubledata[i][4]=Curve_SmallSpeed;
+
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
+                    }
+                }
+
+
+            }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
+
+                doubledata[i][4]=Curve_LargeSpeed;
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
                     }
                 }
+
+            }
+        }
+
+        SmoothACC(MapTrace,doubledata);
     }
+
 //
                 /*for(int i=0;i<MapTrace.size();i++){
                     //将数据写入文件开始

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -376,10 +376,25 @@ void iv::decition::BrainDecition::run() {
     std::string brkdebug = xp.GetParam("brakeDebug","3");
 
 
+
     ServiceCarStatus.ang_debug = atof(angdebug.data());
     ServiceCarStatus.torque_or_acc = atof(tordebug.data());
     ServiceCarStatus.brake = atof(brkdebug.data());
 
+    std::string strbasepitch = xp.GetParam("basepitch","0.0");
+    ServiceCarStatus.mfbasepitch = atof(strbasepitch.data());
+
+    ServiceCarStatus.mbUseRampAssit = xp.GetParam("UseRamp",false);
+
+    gplanbrakeacc = fabs(xp.GetParam("planbrakeacc",0.3));
+    if(gplanbrakeacc <= 0.01)gplanbrakeacc = 0.01;
+
+    gplanacc = fabs(xp.GetParam("planacc",1.0));
+    if(gplanacc <0.1)gplanacc = 0.1;
+
+
+    decitionGps00->mstopbrake = fabs(xp.GetParam("stopbrake",0.6));
+    if(decitionGps00->mstopbrake < 0.1)decitionGps00->mstopbrake = 0.1;
 
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -176,6 +176,9 @@ public:
 //    std::shared_ptr<BasePlanner> mlaneChangePlanner;
 
 
+    double mstopbrake = 0.6;
+
+
     Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
                               std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);