change decition_brain_sf_changan_shenlan, change vertical park use new solution,but not test. change view_ndtmatching.pro, add some shell for 18.04 20.04 22.04 complie.

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

@@ -49,6 +49,7 @@ double  iv::decition::Compute00::dBocheAngle;
 
 double iv::decition::Compute00::mfWheelAngle[5];
 SideParkType iv::decition::Compute00::mSideParkType;
+VerticalParkType iv::decition::Compute00::mVerticalParkType;
 
 
 std::vector<int> lastEsrIdVector;
@@ -2182,6 +2183,38 @@ double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
 }
 
 
+double iv::decition::Compute00::bocheComputeNew(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;
+
+    VerticalParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,ServiceCarStatus.mfMinRadius,ServiceCarStatus.mfMaxWheel,45,ServiceCarStatus.mfParkLastStraightDis);
+    xCalc.CalcPark();
+
+    std::vector<iv::VerticalParkPoint> xvectorverticalparkpoint;
+    std::vector<double> xvectorwheel;
+    std::vector<double> xvectorlen;
+    double fTotalLen = 0;
+    VerticalParkType xtype = xCalc.GetSolution(xvectorverticalparkpoint,xvectorwheel,xvectorlen,fTotalLen);
+
+    if((xtype == VerticalParkType::ThreeStep) && (fTotalLen < ServiceCarStatus.mfParkValidLen))
+    {
+        nearTpoint=Coordinate_UnTransfer(xvectorverticalparkpoint[0].my*(-1), xvectorverticalparkpoint[0].mx, aimGps);
+        farTpoint = Coordinate_UnTransfer(xvectorverticalparkpoint[1].my*(-1), xvectorverticalparkpoint[1].mx, aimGps);
+        for(int i=0;i<3;i++)mfWheelAngle[i] = xvectorwheel[i];
+        mVerticalParkType = VerticalParkType::ThreeStep;
+        return 1;
+    }
+
+
+    return 0;
+
+}
 
 double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
 

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

@@ -9,6 +9,7 @@
 #include <decition/decide_gps_00.h>
 
 #include "sideparkcalc.h"
+#include "verticalparkcalc.h"
 
 namespace iv {
     namespace decition {
@@ -36,6 +37,7 @@ namespace iv {
             static double mfWheelAngle[5];
 
             static SideParkType mSideParkType;
+            static VerticalParkType mVerticalParkType;
 
             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);
@@ -76,6 +78,8 @@ namespace iv {
 
             static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);
 
+            static double bocheComputeNew(GPS_INS nowGps, GPS_INS aimGps);
+
             static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);
 
             static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/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/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/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

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -466,7 +466,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 return gps_decition;
             }else
             {
-                if(trumpet()>3000)
+                if(trumpet()>1000)
                 {
                     trumpetFirstCount=true;
                     vehState=dRever;
@@ -482,7 +482,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr )
         {
-            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            int bocheEN=Compute00().bocheComputeNew(now_gps_ins, aim_gps_ins);
             if(bocheEN==1)
             {
                 ServiceCarStatus.bocheEnable=1;
@@ -510,7 +510,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             }
             else
             {
-                if(trumpet()>3000)
+                if(trumpet()>1000)
                 {
                     trumpetFirstCount=true;
                     vehState=reverseCar;
@@ -567,8 +567,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
         iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
 
-         double fdistonear = fabs(pt.x - ptnear.x);
-         if(fdistonear<0.5)  //将吉利项目中的停车逻辑移植过来
+        double fdistonear = pt.x - ptnear.x;
+        if(ptnear.x < 0) fdistonear = ptnear.x - pt.x;
+         if(fdistonear<0.1)
 //        if (dis<2.0)//0.5
         {
             vehState = reverseCircle;
@@ -593,7 +594,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
         double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
-        if((((angdis<5)||(angdis>355)))&&(dis<3.0))
+        iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
+        iv::Point2D ptfar = iv::decition::Coordinate_Transfer(Compute00().farTpoint.gps_x,Compute00().farTpoint.gps_y,aim_gps_ins);
+        double xx = pt.y - ptfar.y;
+        if((((angdis<2)||(angdis>358)))||(xx<(-0.3)))
             //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
         {
             vehState = reverseDirect;
@@ -602,14 +606,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         }
         else {
-            if (qiedianCount && trumpet()<0)
+            if (qiedianCount && trumpet()<1500)
                 //         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
@@ -626,7 +630,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
             }
 
-            controlAng = Compute00().bocheAngle*16.5;
+            controlAng =  Compute00().mfWheelAngle[1];// Compute00().bocheAngle*16.5;
             gps_decition->wheel_angle = 0 - controlAng*1.05;
 
             cout<<"farTpointDistance================"<<dis<<endl;
@@ -640,7 +644,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;
@@ -658,13 +662,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         else {
 
             //if (qiedianCount && trumpet()<0)
-            if (qiedianCount && trumpet()<1000)
+            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

src/decition/decition_brain_sf_changan_shenlan/decition/decition.pri

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

src/test/testsideparkcalc/mainwindow.cpp

@@ -55,7 +55,7 @@ void MainWindow::SidePark(double x,double y ,double hdg, double fMinRadius,doubl
 {
 
 
-    SideParkCalc xPark(x,y,hdg);
+    SideParkCalc xPark(x,y,hdg,fMinRadius,fMaxWheel,45,fStraightDis);
     xPark.CalcPark();
 
     std::vector<iv::SideParkPoint> xvectorsideparkpoint;

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -70,14 +70,29 @@ LIBS += -lshowxodrinvtk
 INCLUDEPATH += $$PWD/../../common/common/math/
 
 LIBS += -lboost_system
-#LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
-#        -lvtkFiltersSources-6.3
 
-#LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
-#        -lvtkFiltersSources-9.1
 
+system(. /etc/lsb-release;echo $DISTRIB_RELEASE)
+
+
+infile( /etc/lsb-release, DISTRIB_RELEASE, 18.04 ){
+message("system is 22.04")
+}
+
+infile( /etc/lsb-release, DISTRIB_RELEASE, 18.04 ){
+LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
+        -lvtkFiltersSources-6.3
+}
+
+infile( /etc/lsb-release, DISTRIB_RELEASE, 22.04 ){
+LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+        -lvtkFiltersSources-9.1
+}
+
+infile( /etc/lsb-release, DISTRIB_RELEASE, 20.04 ){
 LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
         -lvtkFiltersSources-7.1
+}
 
 FORMS += \
     mainwindow.ui