change decision_brain. changing boche.

src1/decision/common/adc_decision_function/ivpark_simple.cpp

@@ -1,7 +1,10 @@
 #include "ivpark_simple.h"
 
-#include "gps_type.h"
+
 #include "adc_tools/compute_00.h"
+#include "adc_tools/transfer.h"
+
+#include <chrono>
 
 using namespace  iv;
 
@@ -17,6 +20,7 @@ bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
     std::vector<double> xvectorRearDis;
     std::vector<double> xvectorAngle;
     std::vector<int> xvectortype;
+    std::vector<GPS_INS> xvectoraimgps;
     std::vector<iv::simpleparkspace> xvectorsimpleparkspace = GetParkSpace();
     xvectordTPoint.clear();
     xvectorRearDis.clear();
@@ -42,12 +46,20 @@ bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
                 xvectordTPoint.push_back(TPoints);
                 xvectorAngle.push_back(fAngle);
                 xvectorRearDis.push_back(fRearDis);
+                xvectoraimgps.push_back(aimgps);
                 xvectortype.push_back(1);
             }
         }
         if(xpark.mnParkType == 0)
         {
-
+            if(iv::decition::Compute00().bocheCompute(nowgps,aimgps,TPoints,fAngle,fRearDis) == 1)
+            {
+                xvectordTPoint.push_back(TPoints);
+                xvectorAngle.push_back(fAngle);
+                xvectorRearDis.push_back(fRearDis);
+                xvectoraimgps.push_back(aimgps);
+                xvectortype.push_back(0);
+            }
         }
     }
 
@@ -91,6 +103,7 @@ bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
         iv::decition::Compute00().farTpoint = xvectordTPoint[nsel].at(1);
         iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
         iv::decition::Compute00().nParkType = 0;
+        maimgps = xvectoraimgps[nsel];
     }
 
     if(xvectortype[nsel] == 1)
@@ -101,9 +114,265 @@ bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
         iv::decition::Compute00().dTpoint3 = xvectordTPoint[nsel].at(0);
         iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
         iv::decition::Compute00().nParkType = 1;
+        maimgps = xvectoraimgps[nsel];
     }
 
 
 
+    GaussProjCal(maimgps.gps_lng,maimgps.gps_lat,&maimgps.gps_x,&maimgps.gps_y);
     return true;
 }
+
+
+int ivpark_simple::GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode)
+{
+    if(bbocheMode == false)
+    {
+        return 0; //Not in boche mode
+    }
+
+    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    int64_t nstoptime_ms = 1000; //every stop time is 3
+
+    if(xvehstate!=dRever &&  xvehstate!=dRever0  &&  xvehstate!=dRever1 && xvehstate!=dRever2
+                    &&  xvehstate!=dRever3 && xvehstate!=dRever4 &&  xvehstate!=reverseArr
+                    && xvehstate!=reverseCar &&  xvehstate!=reversing  &&  xvehstate!=reverseCircle &&  xvehstate!=reverseDirect)
+    {
+        if(fSpeed>0.3)
+        {
+            fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
+            fAcc = -0.5;
+            fWheel = 0.0;
+            nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        }
+        else
+        {
+            fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
+            fAcc = -0.5;
+            fWheel = 0.0;
+            nshift = 2; //rear
+            int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+            if(abs(nnow - nstoptiming_ms) >= nstoptime_ms)
+            {
+                if(iv::decition::Compute00().nParkType == 0)
+                {
+                    xvehstate = reversing;
+                    mlastvehstate = xvehstate;
+                }
+                else
+                {
+                    xvehstate =  dRever;
+                    mlastvehstate = xvehstate;
+                }
+            }
+
+
+        }
+        return 1;
+    }
+
+    iv::GPS_INS nowgps;
+    nowgps.gps_lat = fLat;
+    nowgps.gps_lng = fLon;
+    GaussProjCal(nowgps.gps_lng,nowgps.gps_lat,&nowgps.gps_x,&nowgps.gps_y);
+
+    switch (xvehstate) {
+    case reversing:
+        reversingcarFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
+    case reverseCircle:
+        reverseCircleFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
+    case reverseDirect:
+        reverseDirectFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
+    case reverseArr:
+        reverseArrFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
+    default:
+        return 0;
+
+    }
+
+}
+
+void ivpark_simple::reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+
+    mlastvehstate = xvehstate;
+
+    double fdistonear = sqrt(pow(nowgps.gps_x - iv::decition::Compute00().nearTpoint.gps_x,2)+pow(nowgps.gps_y - iv::decition::Compute00().nearTpoint.gps_y,2));
+
+    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
+
+    Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().nearTpoint.gps_x,iv::decition::Compute00().nearTpoint.gps_y, maimgps);
+
+    fdistonear = fabs(pt.x - ptnear.x);
+
+    nshift = 2;
+    if(fdistonear>1.0)
+    {
+        fAcc = 0.0;  //acc calcutale by pid
+        fWheel = 0.0;
+        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
+
+    }
+    else
+    {
+        if((fSpeed>0.3)&&(fdistonear>0.3))
+        {
+            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
+        }
+        else
+        {
+                fAcc = -0.5;
+                xvehstate = reverseCircle;
+        }
+    }
+
+
+
+    return;
+}
+
+void ivpark_simple::reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+
+    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    int64_t nstoptime_ms = 1000; //every stop time is 3
+
+    if(mlastvehstate != xvehstate)
+    {
+        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+
+    mlastvehstate = xvehstate;
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+
+    nshift = 2;
+
+    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
+    {
+        fdSpeed = 0;
+        fdSecSpeed = 0;
+        fAcc = -0.5;
+        fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.05);
+        mCircleWheel = fWheel;
+        return;
+
+    }
+
+    fWheel = mCircleWheel;
+
+    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
+    double angdis =fabs(nowgps.ins_heading_angle - maimgps.ins_heading_angle);
+
+    if((fabs(pt.x)<2.0)&&(((angdis<5)||(angdis>355))))
+    {
+        xvehstate = reverseDirect;
+        fAcc = -0.5;
+        fdSpeed = 0.0;
+        fWheel = 0.0;
+        fdSecSpeed = 0.0;
+    }
+    else
+    {
+        fAcc = 0.0;
+        fdSpeed = 2;
+        fdSecSpeed = fdSecSpeed / 3.6;
+    }
+
+    return;
+
+}
+
+void ivpark_simple::reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    int64_t nstoptime_ms = 1000; //every stop time is 3
+
+    if(mlastvehstate != xvehstate)
+    {
+        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+
+    mlastvehstate = xvehstate;
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+
+    nshift = 2;
+
+    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
+    {
+        fdSpeed = 0;
+        fdSecSpeed = 0;
+        fAcc = -0.5;
+        fWheel = 0.0;
+        return;
+
+    }
+
+    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
+
+    if(pt.y<0.5)
+    {
+        xvehstate = reverseArr;
+        fAcc = -0.5;
+        fdSpeed = 0.0;
+        fWheel = 0.0;
+        fdSecSpeed = 0.0;
+    }
+    else
+    {
+        fAcc = 0.0;
+        fdSpeed = 2;
+        fdSecSpeed = fdSecSpeed / 3.6;
+        fWheel = 0.0;
+    }
+
+    return;
+}
+
+void ivpark_simple::reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    int64_t nstoptime_ms = 1000; //every stop time is 3
+
+    if(mlastvehstate != xvehstate)
+    {
+        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+
+    mlastvehstate = xvehstate;
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+
+    nshift = 2;
+
+    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
+    {
+        fdSpeed = 0;
+        fdSecSpeed = 0;
+        fAcc = -0.5;
+        fWheel = 0.0;
+        return;
+
+    }
+
+    nshift = 1; //P shift
+
+    fdSpeed = 0;
+    fdSecSpeed = 0;
+    fAcc = -0.5;
+    fWheel = 0.0;
+
+    xvehstate = reverseComplete;
+}

src1/decision/common/adc_decision_function/ivpark_simple.h

@@ -3,6 +3,9 @@
 
 #include "ivif_park.h"
 
+
+#include "gps_type.h"
+
 class ivpark_simple : public ivif_park
 {
 public:
@@ -11,6 +14,22 @@ public:
 public:
     virtual bool IsBocheEnable(double fLon, double fLat, double fHeading);
 
+    virtual int GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode);
+
+private:
+    iv::GPS_INS maimgps;
+
+    VehState mlastvehstate;
+
+    double mCircleWheel = 0;
+
+private:
+    void reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
+    void reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
+    void reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
+    void reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
+
+
 };
 
 #endif // IVPARK_SIMPLE_H

src1/decision/common/common/car_status.h

@@ -24,8 +24,12 @@
 
 #include "common/roadmode_type.h"
 
+#include "common/comonstruct.h"
+
 #define RADAR_OBJ_NUM 64
 
+
+
 #ifdef linux
 unsigned long GetTickCount();
 #endif

src1/decision/common/common/comonstruct.h

@@ -0,0 +1,10 @@
+#ifndef COMONSTRUCT_H
+#define COMONSTRUCT_H
+
+
+enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
+                waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4,reverseComplete};
+
+
+#endif // COMONSTRUCT_H

src1/decision/decision_brain/decision_brain.pro

@@ -116,7 +116,8 @@ HEADERS += \
     $$PWD/../common/common/vv7.h \
     $$PWD/../common/platform/dataformat.h \
     $$PWD/../common/platform/platform.h \
-    ivdecision_brain.h
+    ivdecision_brain.h \
+    ../common/common/comonstruct.h
 
 INCLUDEPATH += $$PWD/../common
 INCLUDEPATH += $$PWD/../common/common

src1/decision/interface/ivif_park.cpp

@@ -18,6 +18,22 @@ bool ivif_park::IsBocheEnable(double fLon, double fLat, double fHeading)
     return false;
 }
 
+int ivif_park::GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode)
+{
+    (void)fLon;
+    (void)fLat;
+    (void)fHeading;
+    (void)fSpeed;
+    (void)fAcc;
+    (void)fWheel;
+    (void)nshift;
+    (void)fdSpeed;
+    (void)fdSecSpeed;
+    (void)xvehstate;
+    (void)bbocheMode;
+    return 0;
+}
+
 void ivif_park::SetParkSpace(std::vector<iv::simpleparkspace> & xvectorsimpleparkspace)
 {
     mvectorsimpleparkspace = xvectorsimpleparkspace;

src1/decision/interface/ivif_park.h

@@ -3,6 +3,8 @@
 
 #include <vector>
 
+#include "comonstruct.h"
+
 namespace iv
 {
 struct simpleparkspace
@@ -22,8 +24,32 @@ public:
     virtual ~ivif_park();
 
 public:
+    /**
+     * @brief IsBocheEnable
+     * @param fLon 经度
+     * @param fLat 纬度
+     * @param fHeading 航向
+     * @return 是否能泊车
+     */
     virtual bool IsBocheEnable(double fLon, double fLat, double fHeading);
 
+    /**
+     * @brief GetBocheDecision
+     * @param fLon 经度
+     * @param fLat 纬度
+     * @param fHeading 航向
+     * @param fSpeed 车速 km/h
+     * @param fAcc 决策加速度
+     * @param fWheel 决策方向盘角度
+     * @param nshift  2 倒车  3 前进
+     * @param fdSpeed 期望速度 km/h
+     * @param fdSecSpeed 期望速度 m/s
+     * @param xvehstate
+     * @param bbocheMode 是否激活泊车
+     * @return  0 不在泊车状态，无决策   1 在泊车状态，有决策
+     */
+    virtual int GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode);
+
 private:
     std::vector<iv::simpleparkspace> mvectorsimpleparkspace;