change tool_sideparkcalc, calc ok.

src/test/testsideparkcalc/mainwindow.cpp

@@ -12,6 +12,8 @@ MainWindow::MainWindow(QWidget *parent)
     ui->lineEdit_NowX->setText("9.0");
     ui->lineEdit_NowY->setText("3.0");
     ui->lineEdit_NowHdg->setText("0.0");
+
+
 }
 
 MainWindow::~MainWindow()
@@ -28,5 +30,66 @@ void MainWindow::on_pushButton_Calc_clicked()
 
     SideParkCalc xPark(x,y,hdg);
     xPark.CalcPark();
+
+    std::vector<iv::SideParkPoint> xvectorsideparkpoint;
+    std::vector<double> xvectorwheel;
+    std::vector<double> xvectorlen;
+    double fTotalLen = 0;
+    SideParkType xtype = xPark.GetSolution(xvectorsideparkpoint,xvectorwheel,xvectorlen,fTotalLen);
+
+    if(xtype == SideParkType::NoSolution)
+    {
+        ui->plainTextEdit->setPlainText("No Solution.");
+    }
+
+    if(xtype == SideParkType::FiveStep)
+    {
+        char strout[3000];
+        char strtemp[1000];
+        snprintf(strout,3000,"Five Step:\n");
+        snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
+        int i = 0;
+        int  npoint =4;
+        for(i=0;i<npoint;i++)
+        {
+            snprintf(strtemp,1000,"Point %d x:%6.3f y:%6.3f hdg:%6.3f \n",
+                     i,xvectorsideparkpoint[i].mx,xvectorsideparkpoint[i].my,xvectorsideparkpoint[i].mhdg);
+            strncat(strout,strtemp,3000);
+        }
+        snprintf(strtemp,1000,"\nWheel & Len:\n");strncat(strout,strtemp,3000);
+        for(i=0;i<5;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);
+    }
+
+    if(xtype == SideParkType::TwoStep)
+    {
+        char strout[3000];
+        char strtemp[1000];
+        snprintf(strout,3000,"Two Step:\n");
+        snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
+        int i = 0;
+        int  npoint =1;
+        for(i=0;i<npoint;i++)
+        {
+            snprintf(strtemp,1000,"Point %d x:%6.3f y:%6.3f hdg:%6.3f \n",
+                     i,xvectorsideparkpoint[i].mx,xvectorsideparkpoint[i].my,xvectorsideparkpoint[i].mhdg);
+            strncat(strout,strtemp,3000);
+        }
+        snprintf(strtemp,1000,"\nWheel & Len:\n");strncat(strout,strtemp,3000);
+        for(i=0;i<2;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);
+    }
 }
 

src/test/testsideparkcalc/mainwindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>800</width>
-    <height>600</height>
+    <width>1280</width>
+    <height>800</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -17,8 +17,8 @@
    <widget class="QPushButton" name="pushButton_Calc">
     <property name="geometry">
      <rect>
-      <x>270</x>
-      <y>120</y>
+      <x>1010</x>
+      <y>30</y>
       <width>171</width>
       <height>41</height>
      </rect>
@@ -31,7 +31,7 @@
     <property name="geometry">
      <rect>
       <x>123</x>
-      <y>40</y>
+      <y>31</y>
       <width>121</width>
       <height>31</height>
      </rect>
@@ -41,7 +41,7 @@
     <property name="geometry">
      <rect>
       <x>30</x>
-      <y>40</y>
+      <y>32</y>
       <width>91</width>
       <height>31</height>
      </rect>
@@ -53,8 +53,8 @@
    <widget class="QLineEdit" name="lineEdit_NowY">
     <property name="geometry">
      <rect>
-      <x>392</x>
-      <y>40</y>
+      <x>440</x>
+      <y>30</y>
       <width>121</width>
       <height>31</height>
      </rect>
@@ -66,8 +66,8 @@
    <widget class="QLabel" name="label_2">
     <property name="geometry">
      <rect>
-      <x>301</x>
-      <y>40</y>
+      <x>347</x>
+      <y>28</y>
       <width>91</width>
       <height>31</height>
      </rect>
@@ -79,8 +79,8 @@
    <widget class="QLabel" name="label_3">
     <property name="geometry">
      <rect>
-      <x>555</x>
-      <y>40</y>
+      <x>648</x>
+      <y>29</y>
       <width>91</width>
       <height>31</height>
      </rect>
@@ -92,8 +92,8 @@
    <widget class="QLineEdit" name="lineEdit_NowHdg">
     <property name="geometry">
      <rect>
-      <x>647</x>
-      <y>40</y>
+      <x>740</x>
+      <y>30</y>
       <width>121</width>
       <height>31</height>
      </rect>
@@ -102,13 +102,23 @@
      <string/>
     </property>
    </widget>
+   <widget class="QPlainTextEdit" name="plainTextEdit">
+    <property name="geometry">
+     <rect>
+      <x>40</x>
+      <y>100</y>
+      <width>371</width>
+      <height>491</height>
+     </rect>
+    </property>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>800</width>
+     <width>1280</width>
      <height>27</height>
     </rect>
    </property>

src/test/testsideparkcalc/sideparkcalc.cpp

@@ -8,6 +8,9 @@ SideParkCalc::SideParkCalc(double x,double y,double hdg)
     mx = x;
     my = y;
     mhdg =  hdg;
+    normalhdg(mhdg);
+
+    mSideParkType = SideParkType::NoSolution;
 }
 
 
@@ -17,6 +20,12 @@ void SideParkCalc::CalcPark()
     {
         return;
     }
+
+    if(fabs(mhdg) > M_PI/6.0)
+    {
+        return;
+    }
+
     SideParkMode parkmode = CalcParkMode();
 
     if(parkmode == SideParkMode::ParkAtRight)
@@ -93,6 +102,50 @@ void SideParkCalc::ParkAtRightCalc_Model1()
 
     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.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 * (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) )
     //    {
@@ -145,7 +198,7 @@ void SideParkCalc::ParkAtRightCalc_Model2()
     }
     else
     {
-        fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
+        fang = acos(1.0 - (my+dy2fix)/(2.0*mfRaidus)) ;
     }
 
     dx4 = mfRaidus * sin(fang);
@@ -168,6 +221,10 @@ void SideParkCalc::ParkAtRightCalc_Model2()
         {
             dx3 = dy3/k3;
         }
+        else
+        {
+            dx3 = 0;
+        }
 
         if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
         {
@@ -191,12 +248,61 @@ void SideParkCalc::ParkAtRightCalc_Model2()
             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;
     }
 
@@ -260,6 +366,50 @@ void SideParkCalc::ParkAtLeftCalc_Model1()
 
     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));
+
+
+    }
+
 
 
 }
@@ -283,7 +433,7 @@ void SideParkCalc::ParkAtLeftCalc_Model2()
     }
     else
     {
-        fang = acos(1.0 - (y+dy2)/(2.0*mfRaidus)) ;
+        fang = acos(1.0 - (y+dy2fix)/(2.0*mfRaidus)) ;
     }
 
     dx4 = mfRaidus * sin(fang);
@@ -306,8 +456,12 @@ void SideParkCalc::ParkAtLeftCalc_Model2()
         {
             dx3 = dy3/k3;
         }
+        else
+        {
+            dx3 = 0;
+        }
 
-        if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
+        if(fabs(mx - dx1 - dx2 - dx3 -dx4 - dx5) > 0.1)
         {
             std::cout<<" no solve"<<std::endl;
             return;
@@ -330,13 +484,83 @@ void SideParkCalc::ParkAtLeftCalc_Model2()
             if((Rx>0.3) && (fR >= mfRaidus))
             {
                 std::cout<<" Have One Step soluton."<<std::endl;
+
+                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 *(-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/test/testsideparkcalc/sideparkcalc.h

@@ -3,6 +3,7 @@
 
 
 #include <math.h>
+#include <vector>
 
 
 enum SideParkMode
@@ -11,6 +12,31 @@ enum SideParkMode
     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:
@@ -18,11 +44,21 @@ public:
 
 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;
@@ -39,6 +75,8 @@ private:
     void ParkAtLeftCalc();
     void ParkAtLeftCalc_Model1();
     void ParkAtLeftCalc_Model2();
+
+    void normalhdg(double & fhdg);
 };
 
 #endif // SIDEPARKCALC_H

src/tool/simple_planning_simulator/main.cpp

@@ -2,10 +2,19 @@
 
 #include <QApplication>
 
+
+#include <adclicense.h>
+
+
 int main(int argc, char *argv[])
 {
     QApplication a(argc, argv);
     MainWindow w;
     w.show();
-    return a.exec();
+    ADCLicenseServ adclicense;
+    int checklicense=adclicense.CheckLincese();
+    if (checklicense==1)
+    {
+        return a.exec();
+    }
 }

src/tool/simple_planning_simulator/simple_planning_simulator.pro

@@ -106,6 +106,10 @@ INCLUDEPATH += $$PWD/../../common/common
 
 INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
 
+
+INCLUDEPATH += $$PWD/../../common/common/license_local
+include($$PWD/../../common/common/license_local/adclicense.pri)
+
 #unix:LIBS += -lboost_thread -lboost_system -lboost_serialization -lprotobuf
 
 RESOURCES += \

src/tool/view_ndtmatching/main.cpp

@@ -2,6 +2,8 @@
 
 #include <QApplication>
 
+#include <adclicense.h>
+
 #include "xmlparam.h"
 
 
@@ -57,5 +59,10 @@ int main(int argc, char *argv[])
     LoadCameraPos(strpath);
     MainWindow w;
     w.show();
-    return a.exec();
+    ADCLicenseServ adclicense;
+    int checklicense=adclicense.CheckLincese();
+    if (checklicense==1)
+    {
+       return a.exec();
+    }
 }

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -102,4 +102,8 @@ HEADERS += \
     error( "Couldn't find the ivprotobuf.pri file!" )
 }
 
+INCLUDEPATH += $$PWD/../../common/common/license_local
+include($$PWD/../../common/common/license_local/adclicense.pri)
+
+