Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization

src/decition/common/common/car_status.h

@@ -300,6 +300,12 @@ namespace iv {
         double mfWheelRatio  = 13.6; //方向盘角度和轮胎角度的比例
 
 
+        double mfMaxWheel = 430;
+        double mfMinRadius = 4.6;
+        double mfParkLastStraightDis = 0.3;   //When Park, Last Straight Dis;
+        double mfParkValidLen = 30; //If Park
+
+
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

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

@@ -47,6 +47,9 @@ iv::GPS_INS  iv::decition::Compute00::dTpoint2;
 iv::GPS_INS  iv::decition::Compute00::dTpoint3;
 double  iv::decition::Compute00::dBocheAngle;
 
+double iv::decition::Compute00::mfWheelAngle[5];
+SideParkType iv::decition::Compute00::mSideParkType;
+
 
 std::vector<int> lastEsrIdVector;
 std::vector<int> lastEsrCountVector;
@@ -2547,7 +2550,7 @@ int iv::decition::Compute00::bocheDirectComputeNew(GPS_INS nowGps, GPS_INS aimGp
 
     double hdgrel = hdgnow - hdgaim;
 
-    SideParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,4.6,430,45,0.3);
+    SideParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,ServiceCarStatus.mfMinRadius,ServiceCarStatus.mfMaxWheel,45,ServiceCarStatus.mfParkLastStraightDis);
     xCalc.CalcPark();
 
     std::vector<iv::SideParkPoint> xvectorsideparkpoint;
@@ -2557,14 +2560,26 @@ int iv::decition::Compute00::bocheDirectComputeNew(GPS_INS nowGps, GPS_INS aimGp
     SideParkType xtype = xCalc.GetSolution(xvectorsideparkpoint,xvectorwheel,xvectorlen,fTotalLen);
 
 
-    if((xtype == SideParkType::FiveStep) && (fTotalLen < 30.0))
+    if((xtype == SideParkType::FiveStep) && (fTotalLen < ServiceCarStatus.mfParkValidLen))
     {
         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);
+        for(int i=0;i<5;i++)mfWheelAngle[i] = xvectorwheel[i];
+        mSideParkType = SideParkType::FiveStep;
         return 1;
     }
+    else
+    {
+        if((xtype == SideParkType::TwoStep) && (fTotalLen < ServiceCarStatus.mfParkValidLen))
+        {
+            dTpoint0=Coordinate_UnTransfer(xvectorsideparkpoint[0].my*(-1), xvectorsideparkpoint[0].mx, aimGps);
+            for(int i=0;i<2;i++)mfWheelAngle[i] = xvectorwheel[i];
+            mSideParkType = SideParkType::TwoStep;
+            return 1;
+        }
+    }
 
     return 0;
 

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

@@ -33,6 +33,10 @@ namespace iv {
             static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
             static double bocheAngle,dBocheAngle;
 
+            static double mfWheelAngle[5];
+
+            static SideParkType mSideParkType;
+
             static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
             static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
 

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -441,6 +441,10 @@ void iv::decition::BrainDecition::run() {
     ServiceCarStatus.mfWheelBase = xp.GetParam("WheelBase",2.9);
     ServiceCarStatus.mfWheelRatio = xp.GetParam("WheelRatio",13.6);
 
+    ServiceCarStatus.mfMaxWheel = xp.GetParam("MaxWheel",430.0);
+    ServiceCarStatus.mfMinRadius = xp.GetParam("MinRadius",4.6);
+    ServiceCarStatus.mfParkLastStraightDis = xp.GetParam("ParkLastStraightDis",0.3);
+
     if((ServiceCarStatus.msysparam.mvehtype=="shenlan") &&(ServiceCarStatus.mbLimitSpeed))
     {
         ServiceCarStatus.mfMaxSpeed = 20.0;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -727,9 +727,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         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)
+        if(xx < 0.1) //(dis<2.0)
         {
             vehState = dRever1;
+            if(Compute00().mSideParkType == SideParkType::TwoStep)vehState = dRever4;
             qiedianCount = true;
             cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
         }
@@ -738,7 +739,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             controlAng = 0;
             dSpeed = 2;
             dSecSpeed = dSpeed / 3.6;
-            gps_decition->wheel_angle = 0;
+            gps_decition->wheel_angle = (-1.0) * Compute00().mfWheelAngle[0];
             gps_decition->speed = dSpeed;
             obsDistance=-1;
             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
@@ -754,7 +755,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         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)
+        if(xx < 0.1) //(dis<2.0)
         {
             vehState = dRever2;
             qiedianCount = true;
@@ -762,14 +763,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else
         {
-            controlAng =  ServiceCarStatus.mfMaxWheel ;//Compute00().dBocheAngle*16.5;
+            controlAng =  Compute00().mfWheelAngle[1];// ServiceCarStatus.mfMaxWheel ;//Compute00().dBocheAngle*16.5;
             gps_decition->wheel_angle = 0 - controlAng;
             if (qiedianCount && trumpet()<1500)
             {
                 //                gps_decition->brake = 10;
                 //                gps_decition->torque = 0;
                 dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
+                minDecelerate=min(minDecelerate,-1.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
             else
@@ -798,7 +799,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
         double xx= (pt1.y-pt2.y);
         // if(xx>0)
-        if(xx<0.0)
+        if(xx<0.1)
         {
             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);
@@ -817,7 +818,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 /*  gps_decition->brake = 10;
                 gps_decition->torque = 0;  */
                 dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
+                minDecelerate=min(minDecelerate,-1.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
             else
@@ -832,7 +833,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
 
-            gps_decition->wheel_angle = 0 ;
+            gps_decition->wheel_angle = Compute00().mfWheelAngle[2] *(-1.0);//0 ;
             cout<<"farTpointDistance================"<<dis<<endl;
             return gps_decition;
         }
@@ -845,8 +846,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         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)))
+        double xx = pt1.y - pt2.y;
+        if((((angdis<2)||(angdis>358))) || (xx < (-0.3)))
             //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
         // if(xx>0)
   //      if(xx<0.1)
@@ -857,14 +858,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else {
 
-            controlAng =  ServiceCarStatus.mfMaxWheel *(-1.0) ;
+            controlAng =  Compute00().mfWheelAngle[3];//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;
                 dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
+                minDecelerate=min(minDecelerate,-1.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
             else
@@ -899,7 +900,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //            gps_decition->accelerator = -3;
             //            gps_decition->brake =10 ;
             dSpeed=0;
-            minDecelerate=min(minDecelerate,-0.5f);
+            minDecelerate=min(minDecelerate,-1.5f);
             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
 
             gps_decition->wheel_angle=0;
@@ -911,7 +912,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             if (qiedianCount && trumpet()<1500)
             {
                 dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
+                minDecelerate=min(minDecelerate,-1.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
             else

src/test/testsideparkcalc/mainwindow.cpp

@@ -22,6 +22,10 @@ MainWindow::MainWindow(QWidget *parent)
     ui->lineEdit_NowY->setText("3.0");
     ui->lineEdit_NowHdg->setText("0.0");
 
+    ui->lineEdit_MaxWheel->setText("430.0");
+    ui->lineEdit_MinRadius->setText("4.6");
+    ui->lineEdit_StraightDis->setText("0.3");
+
     myview = new MyView(this);
     myview->setObjectName(QStringLiteral("graphicsView"));
     myview->setGeometry(QRect(400, 100,1350, 830));
@@ -38,7 +42,7 @@ MainWindow::MainWindow(QWidget *parent)
     QGraphicsLineItem * pliney = new QGraphicsLineItem(RATIO * X_MOVE,0,RATIO * X_MOVE,VIEW_HEIGHT);
     mpscene->addItem(pliney);
 
-
+    setWindowTitle("Calc Park   (if HDG>0.5236  Vertical Park , else Side Park)");
 }
 
 MainWindow::~MainWindow()
@@ -47,47 +51,9 @@ MainWindow::~MainWindow()
 }
 
 
-void MainWindow::on_pushButton_Calc_clicked()
+void MainWindow::SidePark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis)
 {
-    double x = ui->lineEdit_NowX->text().toDouble();
-    double y = ui->lineEdit_NowY->text().toDouble();
-    double hdg = ui->lineEdit_NowHdg->text().toDouble();
-
-    if(mvectoritem.size()>0)
-    {
-        int i;
-        int nsize = static_cast<int>(mvectoritem.size());
-        for(i=0;i<nsize;i++)
-        {
-            mpscene->removeItem(mvectoritem[i]);
-            delete mvectoritem[i];
-        }
-        mvectoritem.clear();
-
-    }
-
-    QGraphicsEllipseItem * pmainitem;
-    pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
-    pmainitem->setBrush(Qt::green);
-    pmainitem->setPen(Qt::NoPen);
-    double x_main = x * RATIO;
-    double y_main = y * RATIO;
-    double fscale = 1.0;
-    pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
-    pmainitem->setScale(fscale);
-    mpscene->addItem(pmainitem);
-    mvectoritem.push_back(pmainitem);
 
-    pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
-    pmainitem->setBrush(Qt::green);
-    pmainitem->setPen(Qt::NoPen);
-    x_main = 0;
-    y_main = 0;
-    fscale = 1.0;
-    pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
-    pmainitem->setScale(fscale);
-    mpscene->addItem(pmainitem);
-    mvectoritem.push_back(pmainitem);
 
     SideParkCalc xPark(x,y,hdg);
     xPark.CalcPark();
@@ -107,7 +73,7 @@ void MainWindow::on_pushButton_Calc_clicked()
     {
         char strout[3000];
         char strtemp[1000];
-        snprintf(strout,3000,"Five Step:\n");
+        snprintf(strout,3000,"SidePark:\nFive Step:\n");
         snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
         int i = 0;
         int  npoint =4;
@@ -311,7 +277,7 @@ void MainWindow::on_pushButton_Calc_clicked()
     {
         char strout[3000];
         char strtemp[1000];
-        snprintf(strout,3000,"Two Step:\n");
+        snprintf(strout,3000,"SidePark:\nTwo Step:\n");
         snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
         int i = 0;
         int  npoint =1;
@@ -423,3 +389,216 @@ void MainWindow::on_pushButton_Calc_clicked()
     mvectorwheel = xvectorwheel;
 }
 
+void MainWindow::VerticalPark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis)
+{
+    VerticalParkCalc xPark(x,y,hdg,fMinRadius,fMaxWheel,45,fStraightDis);
+    xPark.CalcPark();
+
+    std::vector<iv::VerticalParkPoint> xvectorverticalparkpoint;
+    std::vector<double> xvectorwheel;
+    std::vector<double> xvectorlen;
+    double fTotalLen = 0;
+    VerticalParkType xtype = xPark.GetSolution(xvectorverticalparkpoint,xvectorwheel,xvectorlen,fTotalLen);
+
+    if(xtype == VerticalParkType::NoVSolution)
+    {
+        ui->plainTextEdit->setPlainText("No Solution.");
+    }
+
+    if(xtype == VerticalParkType::ThreeStep)
+    {
+        char strout[3000];
+        char strtemp[1000];
+        snprintf(strout,3000,"VerticalPark:\nThree Steps:\n");
+        snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
+        int i = 0;
+        int  npoint =2;
+        for(i=0;i<npoint;i++)
+        {
+            snprintf(strtemp,1000,"Point %d x:%6.3f y:%6.3f hdg:%6.3f \n",
+                     i,xvectorverticalparkpoint[i].mx,xvectorverticalparkpoint[i].my,xvectorverticalparkpoint[i].mhdg);
+            strncat(strout,strtemp,3000);
+
+
+            QGraphicsEllipseItem * pmainitem;
+            pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
+            pmainitem->setBrush(Qt::red);
+            pmainitem->setPen(Qt::NoPen);
+            double x_main = xvectorverticalparkpoint[i].mx * RATIO;
+            double y_main = xvectorverticalparkpoint[i].my * RATIO;
+            double fscale = 1.0;
+            pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
+            pmainitem->setScale(fscale);
+            mpscene->addItem(pmainitem);
+            mvectoritem.push_back(pmainitem);
+        }
+
+        if(xvectorlen[0]>0)
+        {
+            double x_point,y_point;
+            double fs;
+            fs =0.01;
+            while(fs<xvectorlen[0])
+            {
+                x_point = x + cos(hdg+M_PI)*fs;
+                y_point = y + sin(hdg+M_PI)*fs;
+                fs = fs+ 0.01;
+                QGraphicsEllipseItem * ppointitem;
+                ppointitem = new QGraphicsEllipseItem(0-mfPointSize/2.0,0*(-1)-mfPointSize/2.0,mfPointSize,mfPointSize);
+                ppointitem->setBrush(Qt::green);
+                ppointitem->setPen(Qt::NoPen);
+                double x_main = x_point * RATIO;
+                double y_main = y_point * RATIO;
+                double fscale = 1.0;
+                ppointitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
+                ppointitem->setScale(fscale);
+                mpscene->addItem(ppointitem);
+                mvectoritem.push_back(ppointitem);
+
+            }
+        }
+
+        if(xvectorlen[1]>0)
+        {
+            double x_point,y_point;
+            double fs;
+            fs =0.01;
+            double x_center,y_center;
+            if(xvectorwheel[1]>0)
+            {
+                x_center = xvectorverticalparkpoint[0].mx + mfRadius * cos(xvectorverticalparkpoint[0].mhdg + M_PI/2.0);
+                y_center = xvectorverticalparkpoint[0].my + mfRadius * sin(xvectorverticalparkpoint[0].mhdg + M_PI/2.0);
+            }
+            else
+            {
+                x_center = xvectorverticalparkpoint[0].mx + mfRadius * cos(xvectorverticalparkpoint[0].mhdg - M_PI/2.0);
+                y_center = xvectorverticalparkpoint[0].my + mfRadius * sin(xvectorverticalparkpoint[0].mhdg - M_PI/2.0);
+            }
+            while(fs<xvectorlen[1])
+            {
+                double alpha = xvectorverticalparkpoint[0].mhdg + M_PI/2.0 + fs/mfRadius;
+                if(xvectorwheel[1] >0)
+                {
+                    alpha = xvectorverticalparkpoint[0].mhdg - M_PI/2.0 - fs/mfRadius;
+                }
+
+                x_point = x_center + cos(alpha)*mfRadius;
+                y_point = y_center + sin(alpha)*mfRadius;
+                fs = fs+ 0.01;
+
+                QGraphicsEllipseItem * ppointitem;
+                ppointitem = new QGraphicsEllipseItem(0-mfPointSize/2.0,0*(-1)-mfPointSize/2.0,mfPointSize,mfPointSize);
+                ppointitem->setBrush(Qt::blue);
+                ppointitem->setPen(Qt::NoPen);
+                double x_main = x_point * RATIO;
+                double y_main = y_point * RATIO;
+                double fscale = 1.0;
+                ppointitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
+                ppointitem->setScale(fscale);
+                mpscene->addItem(ppointitem);
+                mvectoritem.push_back(ppointitem);
+
+            }
+        }
+
+        if(xvectorlen[2]>0)
+        {
+            double x_point,y_point;
+            double fs;
+            fs =0.01;
+            while(fs<xvectorlen[2])
+            {
+                x_point = xvectorverticalparkpoint[1].mx + cos(xvectorverticalparkpoint[1].mhdg+M_PI)*fs;
+                y_point = xvectorverticalparkpoint[1].my + sin(xvectorverticalparkpoint[1].mhdg+M_PI)*fs;
+                fs = fs+ 0.01;
+                QGraphicsEllipseItem * ppointitem;
+                ppointitem = new QGraphicsEllipseItem(0-mfPointSize/2.0,0*(-1)-mfPointSize/2.0,mfPointSize,mfPointSize);
+                ppointitem->setBrush(Qt::green);
+                ppointitem->setPen(Qt::NoPen);
+                double x_main = x_point * RATIO;
+                double y_main = y_point * RATIO;
+                double fscale = 1.0;
+                ppointitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
+                ppointitem->setScale(fscale);
+                mpscene->addItem(ppointitem);
+                mvectoritem.push_back(ppointitem);
+
+            }
+        }
+
+
+        snprintf(strtemp,1000,"\nWheel & Len:\n");strncat(strout,strtemp,3000);
+        for(i=0;i<3;i++)
+        {
+            snprintf(strtemp,1000,"Step %d Wheel:%6.3f Length:%6.3f \n",
+                     i,xvectorwheel[i],xvectorlen[i]);
+            strncat(strout,strtemp,3000);
+        }
+        snprintf(strtemp,1000,"\nTotal Length:%6.3f\n",fTotalLen);strncat(strout,strtemp,3000);
+        ui->plainTextEdit->setPlainText(strout);
+    }
+}
+
+
+void MainWindow::on_pushButton_Calc_clicked()
+{
+    double x = ui->lineEdit_NowX->text().toDouble();
+    double y = ui->lineEdit_NowY->text().toDouble();
+    double hdg = ui->lineEdit_NowHdg->text().toDouble();
+
+    double fMinRadius = ui->lineEdit_MinRadius->text().toDouble();
+    mfMaxWheel = ui->lineEdit_MaxWheel->text().toDouble();
+    double fStraightDis = ui->lineEdit_StraightDis->text().toDouble();
+    double fMaxWheel = mfMaxWheel;
+    mfRadius = fMinRadius;
+
+    while(hdg>M_PI)hdg = hdg -2.0*M_PI;
+    while(hdg<=(-M_PI))hdg = hdg + 2.0*M_PI;
+
+
+    if(mvectoritem.size()>0)
+    {
+        int i;
+        int nsize = static_cast<int>(mvectoritem.size());
+        for(i=0;i<nsize;i++)
+        {
+            mpscene->removeItem(mvectoritem[i]);
+            delete mvectoritem[i];
+        }
+        mvectoritem.clear();
+
+    }
+
+    QGraphicsEllipseItem * pmainitem;
+    pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
+    pmainitem->setBrush(Qt::green);
+    pmainitem->setPen(Qt::NoPen);
+    double x_main = x * RATIO;
+    double y_main = y * RATIO;
+    double fscale = 1.0;
+    pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
+    pmainitem->setScale(fscale);
+    mpscene->addItem(pmainitem);
+    mvectoritem.push_back(pmainitem);
+
+    pmainitem = new QGraphicsEllipseItem(0-mfMainSize/2.0,0*(-1)-mfMainSize/2.0,mfMainSize,mfMainSize);
+    pmainitem->setBrush(Qt::green);
+    pmainitem->setPen(Qt::NoPen);
+    x_main = 0;
+    y_main = 0;
+    fscale = 1.0;
+    pmainitem->setPos(X_MOVE * RATIO + x_main,Y_MOVE *RATIO -y_main);
+    pmainitem->setScale(fscale);
+    mpscene->addItem(pmainitem);
+    mvectoritem.push_back(pmainitem);
+
+    if(fabs(hdg)<(M_PI/6.0))
+    {
+        SidePark(x,y,hdg,fMinRadius,fMaxWheel,fStraightDis);
+    }
+    else
+    {
+        VerticalPark(x,y,hdg,fMinRadius,fMaxWheel,fStraightDis);
+    }
+}
+

src/test/testsideparkcalc/mainwindow.h

@@ -6,6 +6,7 @@
 #include "myview.h"
 
 #include "sideparkcalc.h"
+#include "verticalparkcalc.h"
 
 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
@@ -44,5 +45,9 @@ private:
     MyView *myview;
 
     QGraphicsScene * mpscene;
+
+    void SidePark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis);
+    void VerticalPark(double x,double y ,double hdg, double fMinRadius,double fMaxWheel,double fStraightDis);
+
 };
 #endif // MAINWINDOW_H

src/test/testsideparkcalc/mainwindow.ui

@@ -31,7 +31,7 @@
     <property name="geometry">
      <rect>
       <x>123</x>
-      <y>31</y>
+      <y>12</y>
       <width>121</width>
       <height>31</height>
      </rect>
@@ -41,7 +41,7 @@
     <property name="geometry">
      <rect>
       <x>30</x>
-      <y>32</y>
+      <y>15</y>
       <width>91</width>
       <height>31</height>
      </rect>
@@ -54,7 +54,7 @@
     <property name="geometry">
      <rect>
       <x>440</x>
-      <y>30</y>
+      <y>17</y>
       <width>121</width>
       <height>31</height>
      </rect>
@@ -66,8 +66,8 @@
    <widget class="QLabel" name="label_2">
     <property name="geometry">
      <rect>
-      <x>347</x>
-      <y>28</y>
+      <x>350</x>
+      <y>12</y>
       <width>91</width>
       <height>31</height>
      </rect>
@@ -79,8 +79,8 @@
    <widget class="QLabel" name="label_3">
     <property name="geometry">
      <rect>
-      <x>648</x>
-      <y>29</y>
+      <x>650</x>
+      <y>10</y>
       <width>91</width>
       <height>31</height>
      </rect>
@@ -93,7 +93,7 @@
     <property name="geometry">
      <rect>
       <x>740</x>
-      <y>30</y>
+      <y>13</y>
       <width>121</width>
       <height>31</height>
      </rect>
@@ -112,6 +112,75 @@
      </rect>
     </property>
    </widget>
+   <widget class="QLineEdit" name="lineEdit_MinRadius">
+    <property name="geometry">
+     <rect>
+      <x>123</x>
+      <y>60</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_MaxWheel">
+    <property name="geometry">
+     <rect>
+      <x>440</x>
+      <y>60</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_StraightDis">
+    <property name="geometry">
+     <rect>
+      <x>741</x>
+      <y>60</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_4">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>60</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>转弯半径：</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_5">
+    <property name="geometry">
+     <rect>
+      <x>350</x>
+      <y>60</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>转向角度：</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_6">
+    <property name="geometry">
+     <rect>
+      <x>650</x>
+      <y>60</y>
+      <width>91</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>直线距离：</string>
+    </property>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">

src/test/testsideparkcalc/testsideparkcalc.pro

@@ -12,12 +12,14 @@ SOURCES += \
     main.cpp \
     mainwindow.cpp \
     myview.cpp \
-    sideparkcalc.cpp
+    sideparkcalc.cpp \
+    verticalparkcalc.cpp
 
 HEADERS += \
     mainwindow.h \
     myview.h \
-    sideparkcalc.h
+    sideparkcalc.h \
+    verticalparkcalc.h
 
 FORMS += \
     mainwindow.ui

src/test/testsideparkcalc/verticalparkcalc.cpp

@@ -0,0 +1,114 @@
+#include "verticalparkcalc.h"
+
+VerticalParkCalc::VerticalParkCalc(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);
+
+    mVerticalParkType = VerticalParkType::NoVSolution;
+}
+
+
+void VerticalParkCalc::normalhdg(double & fhdg)
+{
+    while(fhdg > M_PI)fhdg = fhdg - 2.0*M_PI;
+    while(fhdg <= (-M_PI))fhdg = fhdg + 2.0*M_PI;
+}
+
+void VerticalParkCalc::CalcPark()
+{
+    if((my>0)&&(mhdg<0))return;
+    if((my<0)&&(mhdg>0))return;
+
+    double fhdg = fabs(mhdg);
+    double x= mx;
+    double y = fabs(my);
+    double dy1,dy2,dy3;
+    dy3 = 0;
+    dy2 = mfRaidus * (1 - cos(fhdg));
+    dy1 = y - dy2;
+
+
+
+    if(fhdg < (M_PI/6.0))return;
+
+    if(dy2>=y)
+    {
+        return;
+    }
+
+    double dx1,dx2,dx3;
+    if(fabs(fhdg - M_PI/2.0) < 1e-6)
+    {
+        dx1 = 0;
+    }
+    else
+    {
+        dx1 =  dy1/tan(fhdg);
+    }
+    dx2 = mfRaidus * sin(fhdg);
+    dx3 = x - dx1 - dx2;
+    if(dx3 < mfLastDirectDis)
+    {
+        return;
+    }
+
+    double x0,y0,x1,y1;
+    x1 = dx3;
+    y1 = dy3;
+    x0 = x1 + dx2;
+    if(my>0)
+        y0 = y1 + dy2;
+    else
+    {
+        y0 = y1 - dy2;
+    }
+
+    mvectorpoint.push_back(iv::VerticalParkPoint(x0,y0,mhdg));
+    mvectorpoint.push_back(iv::VerticalParkPoint(x1,y1,0.0));
+
+    mvectorWheel.push_back(0);
+    if(my>0)
+    {
+        mvectorWheel.push_back(mfMaxWheel);
+    }
+    else
+    {
+        mvectorWheel.push_back(mfMaxWheel*(-1.0));
+    }
+    mvectorWheel.push_back(0.0);
+
+    double flen1,flen2,flen3;
+    flen1 = sqrt(pow(mx - x0,2)+pow(my - y0,2));
+    flen2 = fhdg * mfRaidus;
+    flen3 = dx3;
+    mvectorlen.push_back(flen1);
+    mvectorlen.push_back(flen2);
+    mvectorlen.push_back(flen3);
+    mfTotalLen = flen1 + flen2 + flen3;
+
+    mVerticalParkType = VerticalParkType::ThreeStep;
+
+
+}
+
+VerticalParkType VerticalParkCalc::GetSolution(std::vector<iv::VerticalParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen)
+{
+    if(mVerticalParkType == VerticalParkType::NoVSolution)
+    {
+        return mVerticalParkType;
+    }
+
+    xvectorpoint = mvectorpoint;
+    xvectorWheel = mvectorWheel;
+    xvectorlen = mvectorlen;
+    fTotalLen = mfTotalLen;
+    return mVerticalParkType;
+}

src/test/testsideparkcalc/verticalparkcalc.h

@@ -0,0 +1,63 @@
+#ifndef VERTICALPARKCALC_H
+#define VERTICALPARKCALC_H
+
+
+#include <math.h>
+#include <vector>
+
+
+enum VerticalParkType
+{
+    ThreeStep = 1,  //Wh
+    NoVSolution = 2
+};
+
+namespace iv {
+struct VerticalParkPoint
+{
+    VerticalParkPoint(double x,double y,double fhdg)
+    {
+        mx = x;
+        my = y;
+        mhdg = fhdg;
+    }
+    double mx;
+    double my;
+    double mhdg;
+};
+
+}
+
+
+class VerticalParkCalc
+{
+public:
+    VerticalParkCalc(double x,double y,double hdg,double fRadius = 5.0,double MaxWheel = 430.0,double MaxAngle  = 45.0,double fLastDirectDis = 0.3);
+
+private:
+    double mx,my,mhdg;
+
+    bool mbPark = false;
+
+    double mfLastDirectDis = 0.3;
+    double mfRaidus = 5.0;
+    double mfMaxAngle = 45.0 *M_PI/180.0;
+    double mfMaxWheel = 430.0;
+
+    std::vector<iv::VerticalParkPoint> mvectorpoint;
+    std::vector<double> mvectorWheel; // 3 values
+    std::vector<double> mvectorlen; //3 values
+    double mfTotalLen = 0.0;
+
+    VerticalParkType mVerticalParkType;
+
+private:
+    void normalhdg(double & fhdg);
+
+public:
+    void CalcPark();
+    VerticalParkType GetSolution(std::vector<iv::VerticalParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen);
+
+};
+
+#endif // VERTICALPARKCALC_H