change decition_brain_sf. change sidepark. test ok.

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

@@ -2536,6 +2536,43 @@ int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
 }
 
 
+int iv::decition::Compute00::bocheDirectComputeNew(GPS_INS nowGps, GPS_INS aimGps)
+{
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+
+    double hdgaim = (90 - aimGps.ins_heading_angle) *M_PI/180.0;
+    double hdgnow = (90 - nowGps.ins_heading_angle) *M_PI/180.0;
+
+    double hdgrel = hdgnow - hdgaim;
+
+    SideParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,4.6,430,45,0.3);
+    xCalc.CalcPark();
+
+    std::vector<iv::SideParkPoint> xvectorsideparkpoint;
+    std::vector<double> xvectorwheel;
+    std::vector<double> xvectorlen;
+    double fTotalLen = 0;
+    SideParkType xtype = xCalc.GetSolution(xvectorsideparkpoint,xvectorwheel,xvectorlen,fTotalLen);
+
+
+    if((xtype == SideParkType::FiveStep) && (fTotalLen < 30.0))
+    {
+        dTpoint0=Coordinate_UnTransfer(xvectorsideparkpoint[0].my*(-1), xvectorsideparkpoint[0].mx, aimGps);
+        dTpoint1 = Coordinate_UnTransfer(xvectorsideparkpoint[1].my*(-1), xvectorsideparkpoint[1].mx, aimGps);
+        dTpoint2 = Coordinate_UnTransfer(xvectorsideparkpoint[2].my*(-1), xvectorsideparkpoint[2].mx, aimGps);
+        dTpoint3 = Coordinate_UnTransfer(xvectorsideparkpoint[3].my*(-1), xvectorsideparkpoint[3].mx, aimGps);
+        return 1;
+    }
+
+    return 0;
+
+
+
+}
+
+
 double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
     double L_c = 4.749;//车长
     double rad_1;

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.h

@@ -8,6 +8,8 @@
 #include<vector> 
 #include <decition/decide_gps_00.h>
 
+#include "sideparkcalc.h"
+
 namespace iv {
     namespace decition {
     //根据传感器传输来的信息作出决策
@@ -76,6 +78,9 @@ namespace iv {
 
             static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
 
+            static int bocheDirectComputeNew(GPS_INS nowGps, GPS_INS aimGps);
+
+
             static double min_rad_zhuanxiang(double *R_M, double *min_rad);
             static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
                                     double *x_2, double *y_2, double *real_rad);

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/sideparkcalc.cpp

@@ -0,0 +1,658 @@
+#include "sideparkcalc.h"
+
+#include "math.h"
+#include <iostream>
+
+SideParkCalc::SideParkCalc(double x,double y,double hdg,double fRadius,double MaxWheel ,double MaxAngle ,double fLastDirectDis )
+{
+    mfRaidus = fRadius;
+    mfMaxWheel = MaxWheel;
+    mfMaxAngle = MaxAngle * M_PI/180.0;
+    mfLastDirectDis = fLastDirectDis;
+
+    mx = x;
+    my = y;
+    mhdg =  hdg;
+    normalhdg(mhdg);
+
+    mSideParkType = SideParkType::NoSolution;
+}
+
+
+void SideParkCalc::CalcPark()
+{
+    if(mx< 0.5)
+    {
+        return;
+    }
+
+    if(fabs(mhdg) > M_PI/6.0)
+    {
+        return;
+    }
+
+    SideParkMode parkmode = CalcParkMode();
+
+    if(parkmode == SideParkMode::ParkAtRight)
+    {
+        ParkAtRightCalc();
+    }
+    else
+        ParkAtLeftCalc();
+}
+
+SideParkMode SideParkCalc::CalcParkMode()
+{
+
+
+    if(my>=0)return SideParkMode::ParkAtRight;
+
+    return SideParkMode::ParkAtLeft;
+}
+
+void SideParkCalc::ParkAtRightCalc()
+{
+    if(mhdg<=0.0)
+    {
+        ParkAtRightCalc_Model1();
+    }
+    else
+    {
+        ParkAtRightCalc_Model2();
+    }
+}
+
+void SideParkCalc::ParkAtRightCalc_Model1()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
+    dy6 = 0;
+    dx6 = mfLastDirectDis;
+    //    double fMaxdx5 = mfRaidus * sin(mfMaxAngle );
+    double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+    //    double fMaxdx3 = fMaxdx5;
+    //    double fMaxdy3 = fMaxdy5;
+
+    dx2 = mfRaidus * sin(fabs(mhdg));
+    dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
+
+
+    double fang ;//= mfMaxAngle;
+    if((my+dy2) >= (2*fMaxdy5) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
+    }
+
+    dx5 = mfRaidus * sin(fang);
+    dy5 = mfRaidus * (1.0 - cos(fang));
+
+    dx3 = dx5;
+    dy3 = dy5;
+
+    double thetax = mx - dx6 - dx3 - dx5 - dx2;
+
+    if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
+    {
+        std::cout<<" divide error."<<std::endl;
+        return;
+    }
+    dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg )) + tan(fang ));
+    dx1 = thetax - dx4;
+    dy1 = dx1 * tan(fabs(mhdg));
+    dy4 = my + dy1 +dy2 -dy3-dy5;
+
+    std::cout<<" compute.  "<<std::endl;
+
+    if(dx1>(-0.1)&&(dx4>(-0.1)))
+    {
+        mSideParkType = SideParkType::FiveStep;
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel * (-1.0));
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel);
+        mvectorWheel.push_back(0.0);
+
+        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
+        x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
+        x3 = x4 + dx5;y3 = y4 + dy5;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
+        hdg2 = hdg3;
+        if(dx4>0.01)
+        {
+            x2 = x3 + dx4;y2 = y3 + dy4;
+            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
+        }
+        else
+        {
+            x2 = x3;y2 = y3;flen3 = 0;
+        }
+
+        flen2 = mfRaidus * (fang - mhdg);
+        x1 = mx - dx1;
+        y1 = my + dy1;
+        hdg1 = mhdg;
+
+        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
+
+        mvectorlen.push_back(flen1);
+        mvectorlen.push_back(flen2);
+        mvectorlen.push_back(flen3);
+        mvectorlen.push_back(flen4);
+        mvectorlen.push_back(flen5);
+        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
+        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
+        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
+        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
+        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
+
+
+    }
+
+
+    //    if((my+dy2) > (2*fMaxdy3) )
+    //    {
+    //        dy3 = fMaxdy3;
+    //        dx3 = fMaxdx3;
+    //        dx5 = fMaxdx5;
+    //        dy5 = fMaxdy5;
+    //        double thetax = mx - dx6 - fMaxdx3 - fMaxdx5 - dx2;
+    //        dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - fMaxdy3 - fMaxdy5)/(tan(fabs(mhdg ) + tan(mfMaxAngle )));
+    //        dx1 = thetax - dx4;
+    //        dy1 = dx1 * tan(fabs(mhdg));
+    //        dy4 = my + dy1 + dy2 -dy3-dy5;
+
+    //        std::cout<<" compute.  "<<std::endl;
+    //    }
+    //    else
+    //    {
+    //        double fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
+    //        dx5 = mfRaidus * sin(fang);
+    //        dy5 = mfRaidus * (1.0 - cos(fang));
+
+    //        dx3 = dx5;
+    //        dy3 = dy5;
+
+    //        double thetax = mx - dx6 - dx3 - dx5 - dx2;
+    //        dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
+    //        dx1 = thetax - dx4;
+    //        dy1 = dx1 * tan(fabs(mhdg));
+    //        dy4 = my + dy1 +dy2 -dy3-dy5;
+
+    //        std::cout<<" compute.  "<<std::endl;
+    //     }
+}
+
+void SideParkCalc::ParkAtRightCalc_Model2()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
+    dy5 = 0;
+    dx5 = mfLastDirectDis;
+
+    bool bFirstCalc = true;
+
+    double dx2fix = mfRaidus * sin(mhdg);
+    double dy2fix = mfRaidus * (1.0 - cos(mhdg));
+
+    double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+    double fang ;
+    if((my+dy2fix) >= (2*fMaxdy4) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (my+dy2fix)/(2.0*mfRaidus)) ;
+    }
+
+TwoCalc:
+    dx4 = mfRaidus * sin(fang);
+    dy4 = mfRaidus * (1.0 - cos(fang));
+
+    dx2 = dx4 - dx2fix;
+    dy2 = dy4 - dy2fix;
+
+    double thetax = mx - dx2 -dx4 -dx5;
+
+    double k1 = tan(mhdg);
+    double k3 = tan(fang);
+
+    if(fabs(k1-k3)<0.001)
+    {
+        dy1 = 0;
+        dx1 = 0;
+        dy3 = my - dy2 - dy4;
+        if(k3 > 0.001)
+        {
+            dx3 = dy3/k3;
+        }
+        else
+        {
+            dx3 = 0;
+        }
+
+        if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
+        {
+            std::cout<<" no solve"<<std::endl;
+            return;
+        }
+    }
+    else
+    {
+        dx3 = (my - dy2 - dy4 - k1 * thetax)/(k3 - k1);
+        dy3 = k3 * dx3;
+        dx1 = thetax - dx3;
+        dy1 = k1 * dx1;
+    }
+
+    if((dx3<-0.1) || (dx1<-0.1))
+    {
+        if((dx1>0.5)&&(bFirstCalc))
+        {
+            double x_b = mx - mfLastDirectDis;
+            double y_b = my - 0;
+
+            double k = tan(mhdg);
+            double C = (2 * mfRaidus - dy2fix + k * x_b  - y_b )/(2*mfRaidus);
+
+            double a = 1 +k*k;
+            double b = 2*k*C *(-1.0);
+            double c = C*C-1;
+            double d = b*b - 4*a*c;
+            if(d >= 0)
+            {
+                double xr1,xr2;
+                xr1 = (-b + sqrt(d))/(2*a);
+                xr2 = (-b - sqrt(d))/(2*a);
+                double fa = 0;
+                if((xr1>=0)&&(xr1<1))
+                {
+                    fa = asin(xr1);
+                }
+                else
+                {
+                    if((xr2>=0)&&(xr2<1))
+                    {
+                        fa = asin(xr2);
+                    }
+                }
+
+                if((fa>0.001) && (fa<mfMaxAngle))
+                {
+                    fang = fa;
+                    bFirstCalc = false;
+                    goto TwoCalc;
+                }
+            }
+        }
+        else
+        {
+            if(cos(mhdg)> 0.001)
+            {
+                double fR = my/(1 - cos(mhdg));
+                double Rx = mx - fR*sin(mhdg);
+                if((Rx>0.3) && (fR >= mfRaidus))
+                {
+                    mSideParkType = SideParkType::TwoStep;
+                    mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
+                    mvectorWheel.push_back(0.0);
+
+                    double flen1,flen2;
+                    flen2 = Rx;
+                    flen1 = fR*fabs(mhdg);
+                    mvectorlen.push_back(flen1);
+                    mvectorlen.push_back(flen2);
+                    mfTotalLen = flen1 + flen2;
+                    mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
+                    std::cout<<" Have One Step soluton."<<std::endl;
+                }
+            }
+        }
+    }
+    else
+    {
+        mSideParkType = SideParkType::FiveStep;
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel * (-1.0));
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel);
+        mvectorWheel.push_back(0.0);
+
+        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
+        x4 = dx5;y4 = dy5;hdg4 = 0;flen5 = x4;
+        x3 = x4 + dx4;y3 = y4 + dy4;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
+        hdg2 = hdg3;
+        if(dx3>0.1)
+        {
+            x2 = x3 + dx3;y2 = y3 + dy3;
+            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
+        }
+        else
+        {
+            x2 = x3;y2 = y3;flen3 = 0;
+        }
+
+        flen2 = mfRaidus * (fang - mhdg);
+        x1 = mx - dx1;
+        y1 = my - dy1;
+        hdg1 = mhdg;
+
+        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
+
+        mvectorlen.push_back(flen1);
+        mvectorlen.push_back(flen2);
+        mvectorlen.push_back(flen3);
+        mvectorlen.push_back(flen4);
+        mvectorlen.push_back(flen5);
+        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
+        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
+        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
+        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
+        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
+        std::cout<<" have compute."<<std::endl;
+    }
+
+    std::cout<<" compute. "<<std::endl;
+}
+
+void SideParkCalc::ParkAtLeftCalc()
+{
+    if(mhdg>=0.0)
+    {
+        ParkAtLeftCalc_Model1();
+    }
+    else
+    {
+        ParkAtLeftCalc_Model2();
+    }
+}
+
+void SideParkCalc::ParkAtLeftCalc_Model1()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
+    dy6 = 0;
+    dx6 = mfLastDirectDis;
+
+    double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+
+    dx2 = mfRaidus * sin(fabs(mhdg));
+    dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
+
+ //   double x = mx;
+    double y = fabs(my);
+
+    double fang ;//= mfMaxAngle;
+    if((y+dy2) >= (2*fMaxdy5) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (y+dy2)/(2.0*mfRaidus)) ;
+    }
+
+    dx5 = mfRaidus * sin(fang);
+    dy5 = mfRaidus * (1.0 - cos(fang));
+
+    dx3 = dx5;
+    dy3 = dy5;
+
+    double thetax = mx - dx6 - dx3 - dx5 - dx2;
+
+    if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
+    {
+        std::cout<<" divide error."<<std::endl;
+        return;
+    }
+    dx4 = (y + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg )) + tan(fang ));
+    dx1 = thetax - dx4;
+    dy1 = dx1 * tan(fabs(mhdg));
+    dy4 = y + dy1 +dy2 -dy3-dy5;
+
+    std::cout<<" compute.  "<<std::endl;
+
+    if(dx1>(-0.1)&&(dx4>(-0.1)))
+    {
+        mSideParkType = SideParkType::FiveStep;
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel * (1.0));
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel * (-1.0));
+        mvectorWheel.push_back(0.0);
+
+        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
+        x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
+        x3 = x4 + dx5;y3 = y4 - dy5;hdg3 = fang * (-1);flen4 = fabs(fang) *mfRaidus;
+        hdg2 = hdg3;
+        if(dx4>0.1)
+        {
+            x2 = x3 + dx4;y2 = y3 - dy4;
+            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
+        }
+        else
+        {
+            x2 = x3;y2 = y3;flen3 = 0;
+        }
+
+        flen2 = mfRaidus * (mhdg + fang);
+        x1 = mx - dx1;
+        y1 = my - dy1;
+        hdg1 = mhdg;
+
+        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
+
+        mvectorlen.push_back(flen1);
+        mvectorlen.push_back(flen2);
+        mvectorlen.push_back(flen3);
+        mvectorlen.push_back(flen4);
+        mvectorlen.push_back(flen5);
+        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
+        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
+        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
+        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
+        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
+
+
+    }
+
+
+
+}
+
+void SideParkCalc::ParkAtLeftCalc_Model2()
+{
+    double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
+    dy5 = 0;
+    dx5 = mfLastDirectDis;
+    bool bFirstCalc = true;
+
+    double dx2fix = mfRaidus * sin(fabs(mhdg));
+    double dy2fix = mfRaidus * (1.0 - cos(fabs(mhdg)));
+
+    double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
+
+    double fang ;
+    double y = fabs(my);
+    if((y+dy2fix) >= (2*fMaxdy4) )
+    {
+        fang = mfMaxAngle;
+    }
+    else
+    {
+        fang = acos(1.0 - (y+dy2fix)/(2.0*mfRaidus)) ;
+    }
+TwoCalc:
+
+    dx4 = mfRaidus * sin(fang);
+    dy4 = mfRaidus * (1.0 - cos(fang));
+
+    dx2 = dx4 - dx2fix;
+    dy2 = dy4 - dy2fix;
+
+    double thetax = mx - dx2 -dx4 -dx5;
+
+    double k1 = tan(fabs(mhdg));
+    double k3 = tan(fang);
+
+    if(fabs(k1-k3)<0.001)
+    {
+        dy1 = 0;
+        dx1 = 0;
+        dy3 = y - dy2 - dy4;
+        if(k3 > 0.001)
+        {
+            dx3 = dy3/k3;
+        }
+        else
+        {
+            dx3 = 0;
+        }
+
+        if(fabs(mx - dx1 - dx2 - dx3 -dx4 - dx5) > 0.1)
+        {
+            std::cout<<" no solve"<<std::endl;
+            return;
+        }
+    }
+    else
+    {
+        dx3 = (y - dy2 - dy4 - k1 * thetax)/(k3 - k1);
+        dy3 = k3 * dx3;
+        dx1 = thetax - dx3;
+        dy1 = k1 * dx1;
+    }
+
+    if((dx3<-0.1) || (dx1<-0.1))
+    {
+        if((dx1>0.5)&&(bFirstCalc))
+        {
+            double x_b = mx - mfLastDirectDis;
+            double y_b = y - 0;
+
+            double k = tan(fabs(mhdg));
+            double C = (2 * mfRaidus - dy2fix + k * x_b  - y_b )/(2*mfRaidus);
+
+            double a = 1 +k*k;
+            double b = 2*k*C *(-1.0);
+            double c = C*C-1;
+            double d = b*b - 4*a*c;
+            if(d >= 0)
+            {
+                double xr1,xr2;
+                xr1 = (-b + sqrt(d))/(2*a);
+                xr2 = (-b - sqrt(d))/(2*a);
+                double fa = 0;
+                if((xr1>=0)&&(xr1<1))
+                {
+                    fa = asin(xr1);
+                }
+                else
+                {
+                    if((xr2>=0)&&(xr2<1))
+                    {
+                        fa = asin(xr2);
+                    }
+                }
+
+                if((fa>0.001) && (fa<mfMaxAngle))
+                {
+                    fang = fa;
+                    bFirstCalc = false;
+                    goto TwoCalc;
+                }
+            }
+        }
+        else
+        {
+            if(cos(fabs(mhdg))> 0.001)
+            {
+                double fR = y/(1 - cos(fabs(mhdg)));
+                double Rx = mx - fR*sin(fabs(mhdg));
+                if((Rx>0.3) && (fR >= mfRaidus))
+                {
+                    std::cout<<" Have One Step soluton."<<std::endl;
+
+                    mSideParkType = SideParkType::TwoStep;
+                    mvectorWheel.push_back((-1.0*mfMaxWheel * mfRaidus/fR));
+                    mvectorWheel.push_back(0.0);
+                    double flen1,flen2;
+                    flen2 = Rx;
+                    flen1 = fR*fabs(mhdg);
+                    mvectorlen.push_back(flen1);
+                    mvectorlen.push_back(flen2);
+                    mfTotalLen = flen1 + flen2;
+                    mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
+                    std::cout<<" Have One Step soluton."<<std::endl;
+                }
+            }
+        }
+    }
+    else
+    {
+        mSideParkType = SideParkType::FiveStep;
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel * (1.0));
+        mvectorWheel.push_back(0.0);
+        mvectorWheel.push_back(mfMaxWheel *(-1.0));
+        mvectorWheel.push_back(0.0);
+
+        double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
+        x4 = dx5;y4 = dy5 * (-1.0);hdg4 = 0;flen5 = x4;
+        x3 = x4 + dx4;y3 = y4 - dy4;hdg3 = fang * (-1.0);flen4 = fabs(fang) *mfRaidus;
+        hdg2 = hdg3;
+        if(dx3>0.1)
+        {
+            x2 = x3 + dx3;y2 = y3 - dy3;
+            flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
+        }
+        else
+        {
+            x2 = x3;y2 = y3;flen3 = 0;
+        }
+
+        flen2 = mfRaidus * (fang + mhdg);
+        x1 = mx - dx1;
+        y1 = my + dy1;
+        hdg1 = mhdg;
+
+        flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
+
+        mvectorlen.push_back(flen1);
+        mvectorlen.push_back(flen2);
+        mvectorlen.push_back(flen3);
+        mvectorlen.push_back(flen4);
+        mvectorlen.push_back(flen5);
+        mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
+        mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
+        mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
+        mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
+        mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
+        std::cout<<" have compute."<<std::endl;
+    }
+
+    std::cout<<" compute. "<<std::endl;
+}
+
+void SideParkCalc::normalhdg(double & fhdg)
+{
+    while(fhdg > M_PI)fhdg = fhdg - 2.0*M_PI;
+    while(fhdg <= (-M_PI))fhdg = fhdg + 2.0*M_PI;
+}
+
+SideParkType SideParkCalc::GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen)
+{
+    if(mSideParkType == SideParkType::NoSolution)
+    {
+        return SideParkType::NoSolution;
+    }
+
+    xvectorpoint = mvectorpoint;
+    xvectorWheel = mvectorWheel;
+    xvectorlen = mvectorlen;
+    fTotalLen = mfTotalLen;
+    return mSideParkType;
+}

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/sideparkcalc.h

@@ -0,0 +1,82 @@
+#ifndef SIDEPARKCALC_H
+#define SIDEPARKCALC_H
+
+
+#include <math.h>
+#include <vector>
+
+
+enum SideParkMode
+{
+    ParkAtLeft =1,
+    ParkAtRight =2
+};
+
+enum SideParkType
+{
+    FiveStep = 1,  //Wh
+    TwoStep = 2,  //When hdg is big, y is small, use 2 steps is ok. 1 point
+    NoSolution = 3
+};
+
+
+namespace iv {
+struct SideParkPoint
+{
+    SideParkPoint(double x,double y,double fhdg)
+    {
+        mx = x;
+        my = y;
+        mhdg = fhdg;
+    }
+    double mx;
+    double my;
+    double mhdg;
+};
+
+}
+
+
+class SideParkCalc
+{
+public:
+    SideParkCalc(double x,double y,double hdg,double fRadius = 5.0,double MaxWheel = 430.0,double MaxAngle  = 45.0,double fLastDirectDis = 0.3);
+
+public:
+    void CalcPark();
+    SideParkType GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen);
+
+private:
+    double mfLastDirectDis = 0.3;
+    double mfRaidus = 5.0;
+    double mfMaxAngle = 45.0 *M_PI/180.0;
+    double mfMaxWheel = 430.0;
+
+    std::vector<iv::SideParkPoint> mvectorpoint;
+    SideParkType mSideParkType;   //five steps / 2 steps / no solution
+    std::vector<double> mvectorWheel; // 5 values / 2 values /no value
+    std::vector<double> mvectorlen; //5 values / 2 values /no value
+    double mfTotalLen = 0.0;
+
+
+
+private:
+    double mx,my,mhdg;
+
+    bool mbPark = false;
+
+private:
+    SideParkMode CalcParkMode();
+
+    void ParkAtRightCalc();
+    void ParkAtRightCalc_Model1();
+    void ParkAtRightCalc_Model2();
+
+    void ParkAtLeftCalc();
+    void ParkAtLeftCalc_Model1();
+    void ParkAtLeftCalc_Model2();
+
+    void normalhdg(double & fhdg);
+};
+
+#endif // SIDEPARKCALC_H

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -526,7 +526,9 @@ void iv::decition::BrainDecition::run() {
                 {
                     decition_gps->brake = -2;//保证在进入自动驾驶的时候车辆能够泄压,否则进入自动驾驶车辆无法加速
                 }
-                decition_gps->torque=0.0;
+
+   //             decition_gps->brake = 0;
+                decition_gps->torque=0;
                 decition_gps->acc_active = 0;//ACC扭矩请求激活（为1激活，为0不激活），不激活后不执行扭矩请求，对于深蓝没什么用，因为控制模块直接给的1
                 decition_gps->brake_active = 0;//ACC减速激活（为1制动激活，为0制动不激活）
                 decition_gps->brake_type = 0;//ADC请求类型（制动时为1，其余情况为0）
@@ -536,7 +538,7 @@ void iv::decition::BrainDecition::run() {
                // ServiceCarStatus.ang_debug=ServiceCarStatus.ang_debug*15.9;//如果xml中配置的是车轮转角
                 ServiceCarStatus.ang_debug=max((double)-430.0,ServiceCarStatus.ang_debug);
                 ServiceCarStatus.ang_debug=min((double)430.0,ServiceCarStatus.ang_debug);
-                decition_gps->wheel_angle = ServiceCarStatus.ang_debug;//0;
+                decition_gps->wheel_angle = ServiceCarStatus.ang_debug;//= -430;// ServiceCarStatus.ang_debug;//0;
 
 
                 double dSpeed=200;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -440,7 +440,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2
                 &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr )
         {
-            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+            int bocheEN=Compute00().bocheDirectComputeNew(now_gps_ins, aim_gps_ins);
             if(bocheEN==1){
                 ServiceCarStatus.bocheEnable=1;
             }else if(bocheEN==0){
@@ -716,7 +716,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == dRever)
     {
         GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-        Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+        Compute00().bocheDirectComputeNew(now_gps_ins, aim_gps_ins);
         vehState = dRever0;
         qiedianCount=true;
         std::cout<<"enter side boche mode"<<std::endl;
@@ -724,7 +724,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == dRever0)
     {
         double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
-        if (dis<2.0)
+        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint0.gps_x,Compute00().dTpoint0.gps_y, aim_gps_ins);
+        double xx= (pt1.y-pt2.y);
+        if(xx < 0.3) //(dis<2.0)
         {
             vehState = dRever1;
             qiedianCount = true;
@@ -747,7 +750,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == dRever1)
     {
         double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);
-        if(dis<2.0)
+
+        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint1.gps_x,Compute00().dTpoint1.gps_y, aim_gps_ins);
+        double xx= (pt1.y-pt2.y);
+        if(xx < 0.0) //(dis<2.0)
         {
             vehState = dRever2;
             qiedianCount = true;
@@ -755,9 +762,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else
         {
-            controlAng = Compute00().dBocheAngle*16.5;
+            controlAng =  ServiceCarStatus.mfMaxWheel ;//Compute00().dBocheAngle*16.5;
             gps_decition->wheel_angle = 0 - controlAng;
-            if (qiedianCount && trumpet()<1000)
+            if (qiedianCount && trumpet()<1500)
             {
                 //                gps_decition->brake = 10;
                 //                gps_decition->torque = 0;
@@ -789,9 +796,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
         Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
         Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
-        double xx= (pt1.x-pt2.x);
+        double xx= (pt1.y-pt2.y);
         // if(xx>0)
-        if(xx>-0.5)
+        if(xx<0.0)
         {
             GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
             Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
@@ -805,7 +812,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else
         {
-            if (qiedianCount && trumpet()<1000)
+            if (qiedianCount && trumpet()<1500)
             {
                 /*  gps_decition->brake = 10;
                 gps_decition->torque = 0;  */
@@ -835,8 +842,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == dRever3)
     {
         double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
+        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint3.gps_x,Compute00().dTpoint3.gps_y, aim_gps_ins);
         double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
-        if((((angdis<5)||(angdis>355)))&&(dis<10.0))
+
+        if(((angdis<5)||(angdis>355)))
+            //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
+        // if(xx>0)
+  //      if(xx<0.1)
         {
             vehState = dRever4;
             qiedianCount = true;
@@ -844,9 +857,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else {
 
-            controlAng = 0-Compute00().dBocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng*0.95;
-            if (qiedianCount && trumpet()<1000)
+            controlAng =  ServiceCarStatus.mfMaxWheel *(-1.0) ;
+            gps_decition->wheel_angle =  0 - controlAng;// 0 - controlAng*0.95;
+            if (qiedianCount && trumpet()<1500)
             {
                 //                gps_decition->brake = 10;
                 //                gps_decition->torque = 0;
@@ -878,7 +891,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
         Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
 
-        if ((pt.y)<0.5)
+        if ((pt.y)<0.1)
         {
             qiedianCount=true;
             vehState=reverseArr;
@@ -895,7 +908,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         else
         {
             //if (qiedianCount && trumpet()<0)
-            if (qiedianCount && trumpet()<1000)
+            if (qiedianCount && trumpet()<1500)
             {
                 dSpeed=0;
                 minDecelerate=min(minDecelerate,-0.5f);

src/decition/decition_brain_sf_changan_shenlan/decition/decition.pri

@@ -1,5 +1,6 @@
 HEADERS += \
     $$PWD/adc_adapter/changan_shenlan_adapter.h \
+    $$PWD/adc_tools/sideparkcalc.h \
     $$PWD/decition_type.h \
     $$PWD/decition_maker.h \
     $$PWD/decide_gps_00.h \
@@ -35,6 +36,7 @@ HEADERS += \
 
 SOURCES += \
     $$PWD/adc_adapter/changan_shenlan_adapter.cpp \
+    $$PWD/adc_tools/sideparkcalc.cpp \
     $$PWD/decide_gps_00.cpp \
     $$PWD/brain.cpp \
     $$PWD/decide_line_00_.cpp \

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -106,7 +106,7 @@ HEADERS += \
     ../../include/msgtype/groupmsg.pb.h \
     ../../include/msgtype/brainrunstate.pb.h
 
-#DEFINES += USEAUTOWAREMPC
+DEFINES += USEAUTOWAREMPC
 
 
 if(contains(DEFINES,USEAUTOWAREMPC)){

src/test/testsideparkcalc/sideparkcalc.cpp

@@ -3,8 +3,13 @@
 #include "math.h"
 #include <iostream>
 
-SideParkCalc::SideParkCalc(double x,double y,double hdg)
+SideParkCalc::SideParkCalc(double x,double y,double hdg,double fRadius,double MaxWheel ,double MaxAngle ,double fLastDirectDis )
 {
+    mfRaidus = fRadius;
+    mfMaxWheel = MaxWheel;
+    mfMaxAngle = MaxAngle * M_PI/180.0;
+    mfLastDirectDis = fLastDirectDis;
+
     mx = x;
     my = y;
     mhdg =  hdg;

src/test/testsideparkcalc/sideparkcalc.h

@@ -40,7 +40,7 @@ struct SideParkPoint
 class SideParkCalc
 {
 public:
-    SideParkCalc(double x,double y,double hdg);
+    SideParkCalc(double x,double y,double hdg,double fRadius = 5.0,double MaxWheel = 430.0,double MaxAngle  = 45.0,double fLastDirectDis = 0.3);
 
 public:
     void CalcPark();