change ads_decision.

src1/decision/common/adc_decision_function/ivlocalplan_simple.cpp

@@ -1,6 +1,8 @@
 #include "ivlocalplan_simple.h"
 
 #include <iostream>
+#include "adc_tools/compute_00.h"
+#include "adc_tools/transfer.h"
 
 ivlocalplan_simple::ivlocalplan_simple()
 {
@@ -19,6 +21,8 @@ std::vector<iv::Point2D> ivlocalplan_simple::GetLocalPlan(iv::GPS_INS now_gps_in
     }
 
 
+
+
     return  xvectorplan;
 
 }

src1/decision/common/adc_decision_function/ivnearpoint_simple.cpp

@@ -15,7 +15,7 @@ ivnearpoint_simple::~ivnearpoint_simple()
 }
 
 
-int ivnearpoint_simple::FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
+int ivnearpoint_simple::FindNearPoint(const std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
 {
     iv::GPS_INS now_gps_ins;
     now_gps_ins.gps_lat = fLat;

src1/decision/common/adc_decision_function/ivnearpoint_simple.h

@@ -10,7 +10,7 @@ public:
     ~ivnearpoint_simple();
 
 public:
-    virtual int FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading);
+    virtual int FindNearPoint(const std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading);
     virtual void ResetFind();
 
 private:

src1/decision/common/adc_decision_function/ivpark_simple.cpp

@@ -197,6 +197,18 @@ int ivpark_simple::GetBocheDecision(double fLon,double fLat,double fHeading,doub
     case reverseArr:
         reverseArrFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
         break;
+    case dRever0:
+        dRever0Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
+    case dRever1:
+        dRever1Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
+    case dRever2:
+        dRever2Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
+    case dRever3:
+        dRever3Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
+        break;
     default:
         return 0;
 

src1/decision/common/common/comonstruct.h

@@ -6,5 +6,7 @@ enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoidin
                 waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
                 dRever,dRever0,dRever1,dRever2,dRever3,dRever4,reverseComplete};
 
+enum ShiftState { Drive, NSHIFT, Rear, Park};
+
 
 #endif // COMONSTRUCT_H

src1/decision/decision_brain/ivdecision_brain.cpp

@@ -93,8 +93,61 @@ int ivdecision_brain::getdecision(brain::decition &xdecition,iv::brain::brainsta
         return 0;
     }
 
+
+    double fAcc;
+    double fWheel;
+    int nshift;
+    double fdSpeed;
+    double fdSecSpeed;
+
+    bool bAvoidAvail = true;
+
+    switch (vehState) {
+    case normalRun:
+        break;
+    case preAvoid:
+        break;
+    case avoiding:
+        break;
+    case dRever0:
+        mivpark.GetBocheDecision(now_gps_ins->gps_lng,now_gps_ins->gps_lat,now_gps_ins->ins_heading_angle,
+                                 now_gps_ins->speed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,vehState,true);
+        break;
+    case dRever2:
+        mivpark.GetBocheDecision(now_gps_ins->gps_lng,now_gps_ins->gps_lat,now_gps_ins->ins_heading_angle,
+                                 now_gps_ins->speed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,vehState,true);
+        break;
+    case dRever3:
+        mivpark.GetBocheDecision(now_gps_ins->gps_lng,now_gps_ins->gps_lat,now_gps_ins->ins_heading_angle,
+                                 now_gps_ins->speed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,vehState,true);
+        break;
+    case reversing:
+        mivpark.GetBocheDecision(now_gps_ins->gps_lng,now_gps_ins->gps_lat,now_gps_ins->ins_heading_angle,
+                                 now_gps_ins->speed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,vehState,true);
+        break;
+    case reverseDirect:
+        mivpark.GetBocheDecision(now_gps_ins->gps_lng,now_gps_ins->gps_lat,now_gps_ins->ins_heading_angle,
+                                 now_gps_ins->speed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,vehState,true);
+        break;
+    case reverseCircle:
+        mivpark.GetBocheDecision(now_gps_ins->gps_lng,now_gps_ins->gps_lat,now_gps_ins->ins_heading_angle,
+                                 now_gps_ins->speed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,vehState,true);
+        break;
+    case reverseArr:
+        mivpark.GetBocheDecision(now_gps_ins->gps_lng,now_gps_ins->gps_lat,now_gps_ins->ins_heading_angle,
+                                 now_gps_ins->speed,fAcc,fWheel,nshift,fdSecSpeed,fdSecSpeed,vehState,true);
+        break;
+    default:
+        break;
+
+    }
+
     iv::decition::Decition decition= getDecideFromGPS(*now_gps_ins,mgpsMapLine,lidarptr,xvectorper,xtrafficlight);
 
+    //Park
+    //Local Plan,Selec Path
+    //Controller
+
     xdecition.set_accelerator(decition->accelerator);
     xdecition.set_brake(decition->brake);
     xdecition.set_leftlamp(decition->leftlamp);
@@ -1285,6 +1338,77 @@ void ivdecision_brain::decision_speedctrl(Decition &gps_decition,const std::vect
     std::cout<<"juecesudu2="<<dSpeed<<std::endl;
 }
 
+
+int ivdecision_brain::normalRunDecision(GPS_INS now_gps_ins,
+                                                                   const std::vector<GPSData> gpsMapLine,
+                                                                   iv::LidarGridPtr lidarGridPtr,
+                                                          std::vector<iv::Perception::PerceptionOutput> lidar_per,
+                                                                   iv::TrafficLight trafficLight,
+                                        double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+
+    std::vector<double>  xvectorObs;
+    if(isFirstRun)
+    {
+        xvectorObs.clear();
+    }
+
+    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
+        //  int a=0;
+        fAcc = -0.5;
+        fWheel = 0;
+        nshift = ShiftState::Drive;
+        return 0;
+    }
+
+    PathPoint = mnearpointfunc.FindNearPoint(gpsMapLine,now_gps_ins.gps_lng,now_gps_ins.gps_lat,now_gps_ins.ins_heading_angle);
+
+    if(PathPoint <0)
+    {
+        std::cout<<" Can't Find Near Point."<<std::endl;
+        fAcc = -0.5;
+        fWheel = 0;
+        nshift = ShiftState::Drive;
+        return 0;
+    }
+
+    double fPreviewDis = 100.0;
+
+    int nsize = static_cast<int>(gpsMapLine.size());
+
+    int i;
+    double fDisTotal = 0.0;
+    std::vector<iv::Point2D> xvectorplan;
+    xvectorplan.clear();
+    for(i=PathPoint;i<nsize;i++)
+    {
+        iv::Point2D pt = iv::decition::Coordinate_Transfer(gpsMapLine[i]->gps_x,gpsMapLine[i]->gps_y, now_gps_ins);
+        if(xvectorplan.size()>0)
+        {
+            double fdisadd = sqrt(pow(pt.x - xvectorplan[xvectorplan.size()-1].x,2) + pow(pt.y - xvectorplan[xvectorplan.size()-1].y,2));
+            fDisTotal = fDisTotal + fdisadd;
+        }
+        xvectorplan.push_back(pt);
+        if(fDisTotal >= fPreviewDis)break;
+    }
+
+    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+
+    if(gpsTraceOri.empty())
+    {
+        std::cout<<" No Trace."<<std::endl;
+        fAcc = -0.5;
+        fWheel = 0;
+        nshift = ShiftState::Drive;
+        return 0;
+    }
+
+    lastGpsIndex = PathPoint;
+
+
+}
+
+    iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
 //std::vector<iv::Perception::PerceptionOutput> lidar_per,
 iv::decition::Decition ivdecision_brain::getDecideFromGPS(GPS_INS now_gps_ins,
                                                                    const std::vector<GPSData> gpsMapLine,

src1/decision/decision_brain/ivdecision_brain.h

@@ -27,6 +27,11 @@
 #include "adc_tools/gps_distance.h"
 #include "adc_tools/transfer.h"
 
+#include "common/comonstruct.h"
+
+#include "adc_decision_function/ivpark_simple.h"
+#include "adc_decision_function/ivnearpoint_simple.h"
+
 namespace  iv {
 
 
@@ -34,9 +39,7 @@ namespace  iv {
 class ivdecision_brain : public ivdecision
 {
 public:
-    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} mvehState;
+    enum VehState mvehState;
 public:
     ivdecision_brain();
 
@@ -475,6 +478,15 @@ private:
     void decision_readyParkMode(GPS_INS now_gps_ins,const std::vector<GPSData> & gpsMapLine);
     void decision_speedctrl(iv::decition::Decition & gps_decition,const std::vector<GPSData> & gpsMapLine);
 
+    ivpark_simple mivpark;
+    ivnearpoint_simple mnearpointfunc;
+
+    int normalRunDecision(GPS_INS now_gps_ins,const std::vector<GPSData> gpsMapLine,
+                          iv::LidarGridPtr lidarGridPtr,
+                            std::vector<iv::Perception::PerceptionOutput> lidar_per,
+                          iv::TrafficLight trafficLight,
+                          double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
+
 };
 
 }

src1/decision/interface/ivif_nearpoint.cpp

@@ -10,7 +10,7 @@ ivif_nearpoint::~ivif_nearpoint()
 
 }
 
-int ivif_nearpoint::FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
+int ivif_nearpoint::FindNearPoint(const std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
 {
     (void)gpsMapLine;
     (void)fLon;

src1/decision/interface/ivif_nearpoint.h

@@ -13,7 +13,7 @@ public:
 
 
 public:
-    virtual int FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading);
+    virtual int FindNearPoint(const std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading);
     virtual void ResetFind();
 };