change ecition/decition_brain_sf_changan_shenlan. use kappa.

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

@@ -49,7 +49,27 @@ iv::decition::Decition iv::decition::PIDController::getControlDecition(GPS_INS n
 }
 
 
+double iv::decition::PIDController::CalcKappa(std::vector<Point2D> trace,int preindex)
+{
+    if(trace.size() == 0)return 0;
+    if(preindex>=trace.size())return 0;
+    if(preindex == 0)return 0;
+    double denominator = 2 * trace[preindex].x *(-1);
+    double numerator = pow(trace[preindex].x,2) + pow(trace[preindex].y,2);
+    double fRadius = 1e9;
+    if(fabs(denominator)>0)
+    {
+        fRadius = numerator/denominator;
+    }
+    else
+    {
+        fRadius = 1e9;
+    }
+    if(fRadius == 0)return  0;
+    assert(fRadius != 0);
+    return 1.0/fRadius;
 
+}
 
 float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
     double ang = 0;
@@ -74,6 +94,7 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
         realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
     }
 
+
     //    if(ServiceCarStatus.msysparam.mvehtype=="bus"){
     //        KEang = 14, KEPos = 7, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
     //        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
@@ -90,6 +111,7 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     std::vector<Point2D> farTrace;
 
     int nsize = trace.size();
+
     for (int i = 1; i < nsize-1; i++)
     {
         sumdis += GetDistance(trace[i - 1], trace[i]);
@@ -99,6 +121,18 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
             break;
         }
     }
+
+
+    bool bUseAutowareKappa = true;
+    double wheel_base = 2.9;
+    double wheelratio = 15.6;
+
+    if(bUseAutowareKappa)
+    {
+        double kappa = CalcKappa(trace,gpsIndex);
+        return (-1.0)*kappa * wheel_base * wheelratio * 180.0/M_PI;
+    }
+
     if(gpsIndex == 0)   gpsIndex = nsize -1;
 
     EPos = trace[gpsIndex].x;

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

@@ -54,6 +54,8 @@ namespace iv {
                         float  computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr);
                         float  limiteU(float u ,float ttc);
 
+                        double CalcKappa(std::vector<Point2D> trace,int preindex);
+
 
                     private: