Merge branch 'master' of http://111.33.136.149:3000/adc_pilot/modularization

include/ivpcl.pri

@@ -6,3 +6,4 @@ win32:INCLUDEPATH += $$PWD/../thirdpartylib/pcl/pcl-1.8
 unix:INCLUDEPATH += /usr/include/eigen3
 unix:INCLUDEPATH += /usr/include/pcl-1.7
 unix:INCLUDEPATH += /usr/include/pcl-1.8
+unix:INCLUDEPATH += /usr/include/pcl-1.12

sh/gps_type.h_pro_files.txt

@@ -0,0 +1,19 @@
+decition下的任意工程
+detection_ndt_slam_hcp2
+driver_map_trace
+driver_map_xodrload
+driver_radio_collision
+driver_radio_p900
+data_serials
+ivmapmake
+ivmapmake_sharemem
+map_lanetoxodr
+map_lanetoxodr_graphscene
+tool_mapcreate
+tool_xodrobj
+tracetoxodr
+ui_ads_hmi
+xviz_civetweb
+platform
+v2xTcpClient
+v2xTcpClientWL

src/decition/common/common/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -88,6 +95,9 @@ namespace iv {
         int roadMode = 0;
         int obs_type=0;
 
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
+
 
         Point2D()
        {

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/base_adapter.cpp

@@ -0,0 +1,77 @@
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+
+
+#include <QFile>
+
+iv::decition::BaseAdapter::BaseAdapter(){
+
+    std::vector<std::tuple<double, double, double>> xvectortable_torque,xvectortable_brake;
+    QFile xFile;
+    xFile.setFileName("velacctable.txt");
+    if(xFile.open(QIODevice::ReadOnly))
+    {
+        QByteArray ba = xFile.readAll();
+        QString strba;
+        strba.append(ba);
+        QStringList strlinelist =strba.split("\n");// strba.split(QRegExp("[\t ;]+"));
+        int nline = strlinelist.size();
+        int i;
+        for(i=0;i<nline;i++)
+        {
+            QString str = strlinelist.at(i);
+            str = str.trimmed();
+            QStringList strvaluelist = str.split(QRegExp("[\t ;]+"));
+            if(strvaluelist.size()>=4)
+            {
+                double vel,acc,torque,brake;
+                vel = QString(strvaluelist.at(0)).toDouble();
+                acc = QString(strvaluelist.at(1)).toDouble();
+                torque = QString(strvaluelist.at(2)).toDouble();
+                brake = QString(strvaluelist.at(3)).toDouble();
+                xvectortable_torque.push_back(std::make_tuple(vel,acc,torque));
+                xvectortable_brake.push_back(std::make_tuple(vel,acc,brake));
+            }
+        }
+
+    }
+    xFile.close();
+
+    if(xvectortable_torque.size()>=4)
+    {
+        mInterpolation2D_torque.Init(xvectortable_torque);
+        mInterpolation2D_brake.Init(xvectortable_brake);
+        mbInterpolation2DOK = true;
+    }
+
+}
+
+int iv::decition::BaseAdapter::GetTorqueBrake(double fVeh, double fAcc, double &fTorque, double &fBrake)
+{
+    if(mbInterpolation2DOK == false)return -1;
+    fTorque = mInterpolation2D_torque.Interpolate(std::make_pair(fVeh,fAcc));
+    fBrake = mInterpolation2D_brake.Interpolate(std::make_pair(fVeh,fAcc));
+    return 0;
+}
+
+bool iv::decition::BaseAdapter::IsINterpolationOK()
+{
+    return mbInterpolation2DOK;
+}
+
+
+iv::decition::BaseAdapter::~BaseAdapter(){
+
+}
+
+ iv::decition::Decition  iv::decition::BaseAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  path , float dSpeed, float obsDistacne ,
+                                                                      float  obsSpeed,float accAim,float accNow  , bool changingDangWei, Decition *decition){
+
+
+
+
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/base_adapter.h

@@ -0,0 +1,50 @@
+#ifndef BASE_ADAPTER_H
+#define BASE_ADAPTER_H
+
+
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+
+#include <decition/adc_adapter/interpolation_2d.h>
+
+namespace iv {
+namespace decition {
+
+        class BaseAdapter {
+        public:
+
+            int adapter_id;
+            std::string adapter_name;
+
+            BaseAdapter();
+            ~BaseAdapter();
+
+
+
+        virtual  iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                          float  obsSpeed,float accAim,float accNow, bool changingDangWei,Decition *decition);
+
+
+        private:
+            apollo::control::Interpolation2D mInterpolation2D_torque,mInterpolation2D_brake;
+            bool mbInterpolation2DOK = false;
+
+        public:
+            int GetTorqueBrake(double fVeh,double fAcc,double & fTorque, double & fBrake);
+            bool IsINterpolationOK();
+
+
+        };
+
+    }
+}
+
+
+
+
+
+#endif // BASE_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/bus_adapter.cpp

@@ -0,0 +1,325 @@
+#include <decition/adc_adapter/bus_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+
+using namespace std;
+
+
+iv::decition::BusAdapter::BusAdapter(){
+    adapter_id=4;
+    adapter_name="bus";
+}
+iv::decition::BusAdapter::~BusAdapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::BusAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+    accAim=min(0.7f,accAim);
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+    if (accAim < 0)
+    {
+        controlSpeed=0;
+        controlBrake=u; //102
+        if(obsDistance>0 && obsDistance<10){
+            controlBrake= u*1.5;
+        }
+         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+                 && dSpeed>0 && lastBrake==0){
+            controlBrake=0;
+            controlSpeed=0;
+        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                  && !turn_around ){
+            controlBrake=min(controlBrake,0.3f);
+            controlSpeed=0;
+        }
+        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                && dSpeed>0 && !turn_around ){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+
+        //0110
+        if(changingDangWei){
+            controlBrake=max(0.5f,controlBrake);
+        }
+
+        //斜坡刹车加大 lsn 0217
+        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+            controlBrake=max(2.0f,controlBrake);
+        }
+        //斜坡刹车加大 end
+
+
+
+
+
+    }
+    else
+    {
+        controlBrake = 0;
+
+        if(ServiceCarStatus.torque_st==0){
+            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
+            controlSpeed =min( controlSpeed,2.0f);
+
+        }else{
+            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
+        }
+
+        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
+        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
+        //            if(realSpeed<5  ){
+        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        //            }else if(realSpeed<10){
+        //                controlSpeed=min((float)25.0,controlSpeed);  //40
+        //            }
+        //        }
+
+
+        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
+
+//        if(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
+//            if(controlSpeed>0){
+//                controlSpeed = ServiceCarStatus.torque_st;
+//            }
+//        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
+//            if(controlSpeed>0 ){
+//                controlSpeed = ServiceCarStatus.torque_st;
+//            }
+//        }
+
+
+        if(controlSpeed>ServiceCarStatus.torque_st){
+            controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
+        }else if(controlSpeed<ServiceCarStatus.torque_st){
+            controlSpeed = ServiceCarStatus.torque_st-1.0;
+        }
+
+
+
+
+        //Seizing
+        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
+            seizingCount++;
+        }else{
+            seizingCount=0;
+        }
+        if(seizingCount>=10){
+            controlSpeed=ServiceCarStatus.torque_st+2;
+        }
+
+//        if(controlSpeed==2.0 && ServiceCarStatus.torque_st<2){
+//          controlSpeed=2.4;
+//        }
+        //seizing end
+
+
+
+
+
+
+        // 斜坡加大油门   0217 lsn
+
+        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realSpeed)<1.0){
+            controlSpeed=max((float)20.0,controlSpeed);
+            controlSpeed=min((float)40.0,controlSpeed);
+        }
+        // 斜坡加大油门  end
+
+
+        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realSpeed)<10.0){
+            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        }
+
+    }
+
+    if(controlSpeed>0){
+        controlSpeed=max(controlSpeed,2.4f);
+    }
+
+    //0227 10m nei xianzhi shache
+    if(obsDistance<10 &&obsDistance>0){
+        controlSpeed=0;
+        controlBrake=max(controlBrake,0.8f);
+    }
+
+
+    if(DecideGps00::minDecelerate<0){
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+   // (*decition)->brake = controlBrake*10;
+      (*decition)->brake = controlBrake*9;
+
+    (*decition)->torque=controlSpeed;
+      lastBrake= (*decition)->brake;
+
+
+
+
+
+    lastTorque=(*decition)->torque;
+
+
+
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=1;  // 0: laqi 1: shifang
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    (*decition)->dangweiEnable=true;
+    (*decition)->angleEnable=true;
+    (*decition)->speedEnable=true;
+    (*decition)-> brakeEnable=true;
+    (*decition)->driveMode=1;
+    (*decition)->brakeType=0;
+    (*decition)->angSpeed=60;
+    (*decition)->topLight=0;
+
+
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=2;
+        (*decition)->backLight=1;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=4;
+        (*decition)->backLight=0;
+    }
+
+
+
+    if((*decition)->brake>0){
+        (*decition)->brakeLight=1;
+    }else{
+        (*decition)->brakeLight=0;
+    }
+
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+    (*decition)->handBrake=false;
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
+     (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
+
+
+
+
+        //dangweiTrasfer
+//             if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
+//                 decition_gps->dangWei=2;
+//                 decition_gps->torque=0;
+//             }
+     lastDangWei= (*decition)->dangWei;
+
+            (*decition)->topLight=0; //1116
+            (*decition)->nearLight=0;
+             (*decition)->farLight=0;
+
+
+
+       (*decition)->door=3;
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
+             <<"  decideTorque:"<<(*decition)->torque <<"  decideBrake:"<<(*decition)->brake
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
+
+   (*decition)->accelerator=  (*decition)->torque ;
+    return *decition;
+}
+
+
+
+float iv::decition::BusAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else {
+        BrakeIntMax = 5;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(5.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::BusAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)100.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/bus_adapter.h

@@ -0,0 +1,43 @@
+#ifndef BUS_ADAPTER_H
+#define BUS_ADAPTER_H
+
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include<decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class BusAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        BusAdapter();
+                        ~BusAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+                        int seizingCount=0;
+
+            };
+
+    }
+}
+
+
+#endif // BUS_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/ge3_adapter.cpp

@@ -0,0 +1,190 @@
+#include <decition/adc_adapter/ge3_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+
+using namespace std;
+
+
+iv::decition::Ge3Adapter::Ge3Adapter(){
+    adapter_id=0;
+    adapter_name="ge3";
+}
+iv::decition::Ge3Adapter::~Ge3Adapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::Ge3Adapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                   float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    if(ttc<0){
+        ttc=15;
+    }
+    if (accAim < 0)
+    {
+        if(obsDistance<10 && obsDistance>0 ){
+            if(ttc>3){
+                controlBrake = (int)((0-accAim) * 40.0) + 1;  //(u * 14.0) + 1;
+            }else{
+                controlBrake = (int)((0-accAim) * 60.0) + 1;  //(u * 14.0) + 1;
+            }
+        } else{
+            if(ttc<5){
+                controlBrake = (int)((0-accAim) * 40.0) + 1;  //(u * 14.0) + 1;
+            }else {
+                controlBrake = (int)((0-accAim) * 20.0) + 1;  //(u * 14.0) + 1;
+            }
+        }
+        controlSpeed = 0;
+        if(obsDistance>50 && ttc>8 &&abs(realSpeed-dSpeed)<3){
+            controlBrake=0;
+            controlSpeed=max(lastTorque-10.0,0.0);
+        }
+    }
+    else
+    {
+        controlBrake = 0;
+        if(lastTorque==0){
+            controlSpeed=100;
+        }else{
+            controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+        }
+    }
+
+
+
+    if(ServiceCarStatus.bocheMode){
+        if(dSpeed<6){
+            controlSpeed=min(1.0f,controlSpeed);
+            controlBrake=min(5.0f,controlBrake);
+            if(abs(dSpeed-realSpeed)<2 && controlBrake>0){
+                controlBrake=0;
+            }
+        }
+    }
+
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    if(DecideGps00::minDecelerate<0){
+        controlBrake = max((0-DecideGps00::minDecelerate)*30,controlBrake);
+        controlSpeed=0;
+    }
+    controlBrake=min(controlBrake,100.0f);
+    controlBrake=max(controlBrake,0.0f);
+
+    (*decition)->brake = controlBrake;
+
+    (*decition)->torque= controlSpeed;
+
+
+
+
+
+
+    lastTorque=(*decition)->torque;
+
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=0;
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    if(ServiceCarStatus.bocheMode){
+        (*decition)->dangWei=2;
+    }else{
+        (*decition)->dangWei=1;
+    }
+
+
+    (*decition)->wheel_angle=max((float)-500.0,float((*decition)->wheel_angle+ServiceCarStatus.mfEPSOff));
+    (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
+
+
+    return *decition;
+}
+
+
+
+float iv::decition::Ge3Adapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    int BrakeIntMax = 0;
+    if (realSpeed < 10.0 / 3.6) BrakeIntMax = 40;
+    else if (realSpeed < 20.0 / 3.6) BrakeIntMax = 60;
+    else BrakeIntMax = 100;
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+    if ((obsDistance>0 && obsDistance < 6) || dSpeed == 0)
+    {
+        controlBrake = max(30.0f, controlBrake);
+    }
+    if (obsDistance>0 && obsDistance<10&&obsDistance>0)
+    {
+        controlBrake = max(20.0f, controlBrake);
+    }
+    if (obsDistance<10&&obsDistance>0 &&ttc<8)
+    {
+        controlBrake = max(30.0f, controlBrake);
+    }
+    if (obsDistance<10&&obsDistance>0 &&ttc<5)
+    {
+        controlBrake = max(40.0f, controlBrake);
+    }
+    if (obsDistance<10&&obsDistance>0 &&ttc<1.6)
+    {
+        controlBrake = max(60.0f, controlBrake);
+    }
+    if(obsDistance<5 && obsDistance>0 ){
+        controlBrake = max(60.0f, controlBrake);
+    }
+    if(obsDistance<5 && obsDistance>0 &&ttc<8){
+        controlBrake = max(80.0f, controlBrake);
+    }
+    controlBrake=min(100.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::Ge3Adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+    if(controlBrake>0){
+        controlSpeed=0;
+    }
+
+    if(realSpeed<10){
+        controlSpeed=min((float)400.0,controlSpeed);
+    }else if(realSpeed<30){
+        controlSpeed =min((float)600.0,controlSpeed);
+    }
+    if(controlSpeed>0){
+        if(controlSpeed>lastTorque){
+            controlSpeed=min(float(lastTorque+5.0),controlSpeed);
+        }
+    }
+    if(dSpeed <= 3)
+    {
+        controlSpeed = min(controlSpeed,20.0f);
+    }
+
+    controlSpeed=min((float)1200.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/ge3_adapter.h

@@ -0,0 +1,60 @@
+#ifndef GE3_ADAPTER_H
+#define GE3_ADAPTER_H
+
+
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include<decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class Ge3Adapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+
+                        Ge3Adapter();
+                        ~Ge3Adapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+
+            };
+
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif // GE3_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/hapo_adapter.cpp

@@ -0,0 +1,371 @@
+#include <decition/adc_adapter/hapo_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+using namespace std;
+
+
+iv::decition::HapoAdapter::HapoAdapter(){
+    adapter_id=5;
+    adapter_name="hapo";
+}
+iv::decition::HapoAdapter::~HapoAdapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::HapoAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+    accAim=min(0.7f,accAim);
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+    if (accAim < 0)
+    {
+        controlSpeed=0;
+        controlBrake=u; //102
+        if(obsDistance>0 && obsDistance<10){
+            controlBrake= u*1.0;     //1.5    zj-1.2
+        }
+        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
+            controlBrake=0;
+            controlSpeed=max(0.0,ServiceCarStatus.torque_st-2.0);
+        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+                 && dSpeed>0 && lastBrake==0){
+            controlBrake=0;
+            controlSpeed=0;
+        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                  && !turn_around ){
+            controlBrake=min(controlBrake,0.3f);
+            controlSpeed=0;
+        }
+        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                && dSpeed>0 && !turn_around ){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+
+        //0110
+        if(changingDangWei){
+            controlBrake=max(0.5f,controlBrake);
+        }
+
+        //斜坡刹车加大 lsn 0217
+        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+            controlBrake=max(0.5f,controlBrake);
+        }
+        //斜坡刹车加大 end
+
+
+
+
+
+    }
+    else
+    {
+        controlBrake = 0;
+
+        if(ServiceCarStatus.torque_st==0){
+            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
+            controlSpeed =min( controlSpeed,2.0f);
+
+        }else{
+            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
+        }
+
+        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
+        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
+        //            if(realSpeed<5  ){
+        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        //            }else if(realSpeed<10){
+        //                controlSpeed=min((float)25.0,controlSpeed);  //40
+        //            }
+        //        }
+
+
+        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
+
+//        if(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
+//            if(controlSpeed>0){
+//                controlSpeed = ServiceCarStatus.torque_st;
+//            }
+//        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
+//            if(controlSpeed>0 ){
+//                controlSpeed = ServiceCarStatus.torque_st;
+//            }
+//        }
+
+
+        if(controlSpeed>ServiceCarStatus.torque_st){
+            controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
+        }else if(controlSpeed<ServiceCarStatus.torque_st){
+            controlSpeed = ServiceCarStatus.torque_st-1.0;
+        }
+
+
+
+
+        //Seizing
+        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
+            seizingCount++;
+        }else{
+            seizingCount=0;
+        }
+        if(seizingCount>=10){
+            controlSpeed=ServiceCarStatus.torque_st+2;
+        }
+
+        // 斜坡加大油门   0217 lsn
+
+        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && (dSpeed-realSpeed)>3.0){
+            controlSpeed=max((float)30.0,controlSpeed);
+            controlSpeed=min((float)40.0,controlSpeed);
+        }
+        // 斜坡加大油门  end
+
+
+        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realSpeed)<10.0){
+            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        }
+
+    }
+
+    if(controlSpeed>0){
+        controlSpeed=max(controlSpeed,3.2f);
+    }
+    static bool emergency_brake=false;
+    //0227 10m nei xianzhi shache
+    if(obsDistance<8 &&obsDistance>0){
+        emergency_brake=true;
+    }
+    if(emergency_brake==true){
+        controlSpeed=0;
+        if(realSpeed<6)
+            controlBrake=max(controlBrake,0.5f);//0.8   zj-0.6
+        else
+            controlBrake=max(controlBrake,0.6f);
+        if(obsDistance>12 || (obsDistance==-1)){
+            emergency_brake=false;
+        }
+    }
+
+    if(DecideGps00::minDecelerate<0){
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    double reverse_speed=-(realSpeed+8)/3.6;//avoid veh in the reverse road
+    if((obsDistance>8.0)&&(obsSpeed<reverse_speed)){
+        controlBrake =0;
+        controlSpeed =0;
+        accAim=0;
+    }
+    if(obsDistance>60){
+        if(accAim<-0.5){
+            accAim=max(accAim,-0.5f);
+        }
+    }
+
+
+    (*decition)->brake = controlBrake*9;//9     zj-6
+    (*decition)->torque=controlSpeed;
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
+
+
+
+
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(IsINterpolationOK()))
+    {
+        double ftorque,fbrake;
+        GetTorqueBrake(now_gps_ins.speed, accAim,ftorque,fbrake);
+        if(accAim>0){
+            ftorque=max(ftorque,3.2);
+        }
+        (*decition)->brake = fbrake;
+
+        (*decition)->torque= ftorque;
+    }
+//    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
+//                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed);
+
+//    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f",
+//                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque);
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=1;  // 0: laqi 1: shifang
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    (*decition)->dangweiEnable=true;
+    (*decition)->angleEnable=true;
+    (*decition)->speedEnable=true;
+    (*decition)-> brakeEnable=true;
+    (*decition)->air_enable = false;
+    (*decition)->driveMode=1;
+    (*decition)->brakeType=0;
+    (*decition)->angSpeed=60;
+    (*decition)->topLight=0;
+
+
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=2;
+        (*decition)->backLight=1;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=4;
+        (*decition)->backLight=0;
+    }
+
+
+
+    if((*decition)->brake>0){
+        (*decition)->brakeLight=1;
+    }else{
+        (*decition)->brakeLight=0;
+    }
+
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+    (*decition)->handBrake=false;
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
+    (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
+    lastDangWei= (*decition)->dangWei;
+
+    (*decition)->topLight=0; //1116
+    (*decition)->nearLight=0;
+    (*decition)->farLight=0;
+    (*decition)->home_light=0;
+    (*decition)->roof_light=0;
+
+     static int64_t DoorTimeStart=-1;
+     static int32_t door=0;
+     if(ServiceCarStatus.has_mbdoor){
+         if(ServiceCarStatus.mbdoor){
+             door=2;
+             //(*decition)->door=2;
+             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
+             ServiceCarStatus.has_mbdoor=0;
+         }else{
+             door=3;
+            // (*decition)->door=3;
+             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
+             ServiceCarStatus.has_mbdoor=0;
+         }
+     }
+
+     if((QDateTime::currentSecsSinceEpoch()-DoorTimeStart)>10)
+             (*decition)->door=0;
+     else
+              (*decition)->door=door;
+
+
+     if(ServiceCarStatus.has_mbjinguang){
+         if(ServiceCarStatus.mbjinguang){
+             (*decition)->nearLight=1;
+         }else{
+             (*decition)->nearLight=0;
+         }
+     }
+
+//std::cout<<"==========================================="<<std::endl;
+//     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
+//             <<"  decideTorque:"<<(*decition)->torque <<"  decideBrake:"<<(*decition)->brake
+//    <<std::endl;
+//std::cout<<"========================================="<<std::endl;
+
+   (*decition)->accelerator=  (*decition)->torque ;
+    return *decition;
+}
+
+
+
+float iv::decition::HapoAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else {
+        BrakeIntMax = 5;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(5.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::HapoAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)100.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/hapo_adapter.h

@@ -0,0 +1,43 @@
+#ifndef HAPO_ADAPTER_H
+#define HAPO_ADAPTER_H
+
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class HapoAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        HapoAdapter();
+                        ~HapoAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+                        int seizingCount=0;
+
+            };
+
+    }
+}
+
+
+#endif // HAPO_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/interpolation_2d.cc

@@ -0,0 +1,120 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "interpolation_2d.h"
+
+#include <cmath>
+#include <iostream>
+
+//#include "cyber/common/log.h"
+
+namespace {
+
+const double kDoubleEpsilon = 1.0e-6;
+
+}  // namespace
+
+namespace apollo {
+namespace control {
+
+bool Interpolation2D::Init(const DataType &xyz) {
+  if (xyz.empty()) {
+    std::cout << "empty input."<<std::endl;
+    return false;
+  }
+  for (const auto &t : xyz) {
+    xyz_[std::get<0>(t)][std::get<1>(t)] = std::get<2>(t);
+  }
+  return true;
+}
+
+double Interpolation2D::Interpolate(const KeyType &xy) const {
+  double max_x = xyz_.rbegin()->first;
+  double min_x = xyz_.begin()->first;
+  if (xy.first >= max_x - kDoubleEpsilon) {
+    return InterpolateYz(xyz_.rbegin()->second, xy.second);
+  }
+  if (xy.first <= min_x + kDoubleEpsilon) {
+    return InterpolateYz(xyz_.begin()->second, xy.second);
+  }
+
+  auto itr_after = xyz_.lower_bound(xy.first);
+  auto itr_before = itr_after;
+  if (itr_before != xyz_.begin()) {
+    --itr_before;
+  }
+
+  double x_before = itr_before->first;
+  double z_before = InterpolateYz(itr_before->second, xy.second);
+  double x_after = itr_after->first;
+  double z_after = InterpolateYz(itr_after->second, xy.second);
+
+  double x_diff_before = std::fabs(xy.first - x_before);
+  double x_diff_after = std::fabs(xy.first - x_after);
+
+  return InterpolateValue(z_before, x_diff_before, z_after, x_diff_after);
+}
+
+double Interpolation2D::InterpolateYz(const std::map<double, double> &yz_table,
+                                      double y) const {
+  if (yz_table.empty()) {
+    std::cout << "Unable to interpolateYz because yz_table is empty."<<std::endl;
+    return y;
+  }
+  double max_y = yz_table.rbegin()->first;
+  double min_y = yz_table.begin()->first;
+  if (y >= max_y - kDoubleEpsilon) {
+    return yz_table.rbegin()->second;
+  }
+  if (y <= min_y + kDoubleEpsilon) {
+    return yz_table.begin()->second;
+  }
+
+  auto itr_after = yz_table.lower_bound(y);
+  auto itr_before = itr_after;
+
+  if (itr_before != yz_table.begin()) {
+    --itr_before;
+  }
+
+  double y_before = itr_before->first;
+  double z_before = itr_before->second;
+  double y_after = itr_after->first;
+  double z_after = itr_after->second;
+
+  double y_diff_before = std::fabs(y - y_before);
+  double y_diff_after = std::fabs(y - y_after);
+
+  return InterpolateValue(z_before, y_diff_before, z_after, y_diff_after);
+}
+
+double Interpolation2D::InterpolateValue(const double value_before,
+                                         const double dist_before,
+                                         const double value_after,
+                                         const double dist_after) const {
+  if (dist_before < kDoubleEpsilon) {
+    return value_before;
+  }
+  if (dist_after < kDoubleEpsilon) {
+    return value_after;
+  }
+  double value_gap = value_after - value_before;
+  double value_buff = value_gap * dist_before / (dist_before + dist_after);
+  return value_before + value_buff;
+}
+
+}  // namespace control
+}  // namespace apollo

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/interpolation_2d.h

@@ -0,0 +1,73 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#pragma once
+
+#include <map>
+#include <memory>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+/**
+ * @namespace apollo::control
+ * @brief apollo::control
+ */
+namespace apollo {
+namespace control {
+/**
+ * @class Interpolation2D
+ *
+ * @brief linear interpolation from key (double, double) to one double value.
+ */
+class Interpolation2D {
+ public:
+  typedef std::vector<std::tuple<double, double, double>> DataType;
+  typedef std::pair<double, double> KeyType;
+
+  Interpolation2D() = default;
+
+  /**
+   * @brief initialize Interpolation2D internal table
+   * @param xyz passing interpolation initialization table data
+   * @return true if init is ok.
+   */
+  bool Init(const DataType &xyz);
+
+  /**
+   * @brief linear interpolate from 2D key (double, double) to one double value.
+   * @param xyz passing interpolation initialization table data
+   * @return true if init is ok.
+   */
+  double Interpolate(const KeyType &xy) const;
+
+ private:
+  double InterpolateYz(const std::map<double, double> &yz_table,
+                       double y) const;
+
+  double InterpolateValue(const double value_before, const double dist_before,
+                          const double value_after,
+                          const double dist_after) const;
+
+  std::map<double, std::map<double, double>> xyz_;
+};
+
+}  // namespace control
+}  // namespace apollo

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/qingyuan_adapter.cpp

@@ -0,0 +1,312 @@
+#include <decition/adc_adapter/qingyuan_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+
+using namespace std;
+
+
+iv::decition::QingYuanAdapter::QingYuanAdapter(){
+    adapter_id=1;
+    adapter_name="qingyuan";
+}
+iv::decition::QingYuanAdapter::~QingYuanAdapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::QingYuanAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+    if (accAim < 0)
+    {
+        controlSpeed=0;
+        controlBrake=u; //102
+        if(obsDistance>0 && obsDistance<10){
+            controlBrake= u*1.5;
+        }
+        if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
+            controlBrake=0;
+            controlSpeed=max(0.0,ServiceCarStatus.torque_st-10.0);
+        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
+            controlBrake=0;
+            controlSpeed=0;
+        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+        else if(abs(dSpeed-realSpeed)>=3 &&abs(dSpeed-realSpeed)<5&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                && dSpeed>0 && !turn_around ){
+            controlBrake=0;
+            controlSpeed=0;
+        }else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                 && dSpeed>0 && !turn_around ){
+            controlBrake=min(controlBrake,0.3f);
+            controlSpeed=0;
+        }
+        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                && dSpeed>0 && !turn_around ){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+
+        //0110
+        if(changingDangWei){
+            controlBrake=max(0.5f,controlBrake);
+        }
+
+        //斜坡刹车加大 lsn 0217
+        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+            controlBrake=max(2.0f,controlBrake);
+        }
+        //斜坡刹车加大 end
+
+
+
+
+
+    }
+    else
+    {
+        controlBrake = 0;
+
+        if(ServiceCarStatus.torque_st==0){
+            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
+            controlSpeed =min( controlSpeed,1.0f);
+
+        }else{
+            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*10;//1115    *10
+        }
+
+        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
+        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
+        //            if(realSpeed<5  ){
+        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        //            }else if(realSpeed<10){
+        //                controlSpeed=min((float)25.0,controlSpeed);  //40
+        //            }
+        //        }
+
+
+        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
+        if(controlSpeed>ServiceCarStatus.torque_st){
+            controlSpeed = ServiceCarStatus.torque_st+1.0;
+        }else if(controlSpeed<ServiceCarStatus.torque_st){
+            controlSpeed = ServiceCarStatus.torque_st-1.0;
+        }
+
+
+
+
+
+
+        //Seizing
+        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1&&realSpeed<0.5){
+            seizingCount++;
+        }else{
+            seizingCount=0;
+        }
+        if(seizingCount>=10){
+            controlSpeed=ServiceCarStatus.torque_st+2;
+        }
+        //seizing end
+
+
+
+
+        // 斜坡加大油门   0217 lsn
+
+        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realSpeed)<1.0){
+            controlSpeed=max((float)20.0,controlSpeed);
+            controlSpeed=min((float)40.0,controlSpeed);
+        }
+        // 斜坡加大油门  end
+
+
+        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realSpeed)<10.0){
+            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        }
+
+    }
+
+    //0227 10m nei xianzhi shache
+    if(obsDistance<10 &&obsDistance>0){
+        controlSpeed=0;
+        controlBrake=max(controlBrake,0.8f);
+    }
+
+
+    if(DecideGps00::minDecelerate<0){
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    (*decition)->brake = controlBrake;
+
+    (*decition)->torque=controlSpeed;
+
+
+
+
+
+
+    lastTorque=(*decition)->torque;
+
+
+
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=1;  // 0: laqi 1: shifang
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    (*decition)->dangweiEnable=true;
+    (*decition)->angleEnable=true;
+    (*decition)->speedEnable=true;
+    (*decition)-> brakeEnable=true;
+    (*decition)->driveMode=1;
+    (*decition)->brakeType=0;
+    (*decition)->angSpeed=60;
+    (*decition)->topLight=0;
+
+
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=3;
+        (*decition)->backLight=1;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=4;
+        (*decition)->backLight=0;
+    }
+
+
+
+    if((*decition)->brake>0){
+        (*decition)->brakeLight=1;
+    }else{
+        (*decition)->brakeLight=0;
+    }
+
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-570.0,(*decition)->wheel_angle);
+     (*decition)->wheel_angle=min((float)570.0,(*decition)->wheel_angle);
+
+
+
+
+        //dangweiTrasfer
+//             if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
+//                 decition_gps->dangWei=2;
+//                 decition_gps->torque=0;
+//             }
+     lastDangWei= (*decition)->dangWei;
+
+            (*decition)->topLight=0; //1116
+            (*decition)->nearLight=0;
+             (*decition)->farLight=0;
+
+
+
+       (*decition)->door=3;
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
+             <<"  decideTorque:"<<(*decition)->torque
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
+    return *decition;
+}
+
+
+
+float iv::decition::QingYuanAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else if(ttc>1.6){
+        BrakeIntMax = 5;
+    }else if(ttc>0.8){
+        BrakeIntMax = 8;
+    }else{
+        BrakeIntMax = 10;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(10.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::QingYuanAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)100.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/qingyuan_adapter.h

@@ -0,0 +1,41 @@
+#ifndef QINGYUAN_ADAPTER_H
+#define QINGYUAN_ADAPTER_H
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include<decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class QingYuanAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+                        int seizingCount=0;
+
+                        QingYuanAdapter();
+                        ~QingYuanAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+
+            };
+
+    }
+}
+
+
+#endif // QINGYUAN_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/sightseeing_adapter.cpp

@@ -0,0 +1,371 @@
+#include <decition/adc_adapter/sightseeing_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+using namespace std;
+
+
+iv::decition::SightseeingAdapter::SightseeingAdapter(){
+    adapter_id=6;
+    adapter_name="sightseeing";
+}
+iv::decition::SightseeingAdapter::~SightseeingAdapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::SightseeingAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+    accAim=min(0.7f,accAim);
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+    if (accAim < 0)
+    {
+        controlSpeed=0;
+        controlBrake=u; //102
+        if(obsDistance>0 && obsDistance<10){
+            controlBrake= u*1.0;     //1.5    zj-1.2
+        }
+        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around){
+            controlBrake=0;
+            controlSpeed=max(0.0,ServiceCarStatus.torque_st-2.0);
+        }else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+                 && dSpeed>0 && lastBrake==0){
+            controlBrake=0;
+            controlSpeed=0;
+        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                  && !turn_around ){
+            controlBrake=min(controlBrake,0.3f);
+            controlSpeed=0;
+        }
+        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+                && dSpeed>0 && !turn_around ){
+            controlBrake=min(controlBrake,0.5f);
+            controlSpeed=0;
+        }
+
+
+        //0110
+        if(changingDangWei){
+            controlBrake=max(0.5f,controlBrake);
+        }
+
+        //斜坡刹车加大 lsn 0217
+        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+            controlBrake=max(0.5f,controlBrake);
+        }
+        //斜坡刹车加大 end
+
+
+
+
+
+    }
+    else
+    {
+        controlBrake = 0;
+
+        if(ServiceCarStatus.torque_st==0){
+            controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
+            controlSpeed =min( controlSpeed,2.0f);
+
+        }else{
+            controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115    *10
+        }
+
+        //        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
+        //            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
+        //            if(realSpeed<5  ){
+        //                controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        //            }else if(realSpeed<10){
+        //                controlSpeed=min((float)25.0,controlSpeed);  //40
+        //            }
+        //        }
+
+
+        //   controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
+
+//        if(accAim>0 && ServiceCarStatus.mfCalAcc>0  && realSpeed>1 ){
+//            if(controlSpeed>0){
+//                controlSpeed = ServiceCarStatus.torque_st;
+//            }
+//        }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
+//            if(controlSpeed>0 ){
+//                controlSpeed = ServiceCarStatus.torque_st;
+//            }
+//        }
+
+
+        if(controlSpeed>ServiceCarStatus.torque_st){
+            controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
+        }else if(controlSpeed<ServiceCarStatus.torque_st){
+            controlSpeed = ServiceCarStatus.torque_st-1.0;
+        }
+
+
+
+
+        //Seizing
+        if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
+            seizingCount++;
+        }else{
+            seizingCount=0;
+        }
+        if(seizingCount>=10){
+            controlSpeed=ServiceCarStatus.torque_st+2;
+        }
+
+        // 斜坡加大油门   0217 lsn
+
+        if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && (dSpeed-realSpeed)>3.0){
+            controlSpeed=max((float)30.0,controlSpeed);
+            controlSpeed=min((float)40.0,controlSpeed);
+        }
+        // 斜坡加大油门  end
+
+
+        else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+                 (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realSpeed)<10.0){
+            controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao  30
+        }
+
+    }
+
+    if(controlSpeed>0){
+        controlSpeed=max(controlSpeed,3.2f);
+    }
+    static bool emergency_brake=false;
+    //0227 10m nei xianzhi shache
+    if(obsDistance<8 &&obsDistance>0){
+        emergency_brake=true;
+    }
+    if(emergency_brake==true){
+        controlSpeed=0;
+        if(realSpeed<6)
+            controlBrake=max(controlBrake,0.5f);//0.8   zj-0.6
+        else
+            controlBrake=max(controlBrake,0.6f);
+        if(obsDistance>12 || (obsDistance==-1)){
+            emergency_brake=false;
+        }
+    }
+
+    if(DecideGps00::minDecelerate<0){
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    double reverse_speed=-(realSpeed+8)/3.6;//avoid veh in the reverse road
+    if((obsDistance>8.0)&&(obsSpeed<reverse_speed)){
+        controlBrake =0;
+        controlSpeed =0;
+        accAim=0;
+    }
+    if(obsDistance>60){
+        if(accAim<-0.5){
+            accAim=max(accAim,-0.5f);
+        }
+    }
+
+
+    (*decition)->brake = controlBrake*9;//9     zj-6
+    (*decition)->torque=controlSpeed;
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
+
+
+
+
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(IsINterpolationOK()))
+    {
+        double ftorque,fbrake;
+        GetTorqueBrake(now_gps_ins.speed, accAim,ftorque,fbrake);
+        if(accAim>0){
+            ftorque=max(ftorque,3.2);
+        }
+        (*decition)->brake = fbrake;
+
+        (*decition)->torque= ftorque;
+    }
+//    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
+//                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed);
+
+//    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f",
+//                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque);
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=1;  // 0: laqi 1: shifang
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    (*decition)->dangweiEnable=true;
+    (*decition)->angleEnable=true;
+    (*decition)->speedEnable=true;
+    (*decition)-> brakeEnable=true;
+    (*decition)->air_enable = false;
+    (*decition)->driveMode=1;
+    (*decition)->brakeType=0;
+    (*decition)->angSpeed=60;
+    (*decition)->topLight=0;
+
+
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=2;
+        (*decition)->backLight=1;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=4;
+        (*decition)->backLight=0;
+    }
+
+
+
+    if((*decition)->brake>0){
+        (*decition)->brakeLight=1;
+    }else{
+        (*decition)->brakeLight=0;
+    }
+
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+    (*decition)->handBrake=false;
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
+    (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
+    lastDangWei= (*decition)->dangWei;
+
+    (*decition)->topLight=0; //1116
+    (*decition)->nearLight=0;
+    (*decition)->farLight=0;
+    (*decition)->home_light=0;
+    (*decition)->roof_light=0;
+
+     static int64_t DoorTimeStart=-1;
+     static int32_t door=0;
+     if(ServiceCarStatus.has_mbdoor){
+         if(ServiceCarStatus.mbdoor){
+             door=2;
+             //(*decition)->door=2;
+             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
+             ServiceCarStatus.has_mbdoor=0;
+         }else{
+             door=3;
+            // (*decition)->door=3;
+             DoorTimeStart=QDateTime::currentSecsSinceEpoch();
+             ServiceCarStatus.has_mbdoor=0;
+         }
+     }
+
+     if((QDateTime::currentSecsSinceEpoch()-DoorTimeStart)>10)
+             (*decition)->door=0;
+     else
+              (*decition)->door=door;
+
+
+     if(ServiceCarStatus.has_mbjinguang){
+         if(ServiceCarStatus.mbjinguang){
+             (*decition)->nearLight=1;
+         }else{
+             (*decition)->nearLight=0;
+         }
+     }
+
+//std::cout<<"==========================================="<<std::endl;
+//     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
+//             <<"  decideTorque:"<<(*decition)->torque <<"  decideBrake:"<<(*decition)->brake
+//    <<std::endl;
+//std::cout<<"========================================="<<std::endl;
+
+   (*decition)->accelerator=  (*decition)->torque ;
+    return *decition;
+}
+
+
+
+float iv::decition::SightseeingAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else {
+        BrakeIntMax = 5;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(5.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::SightseeingAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)100.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/sightseeing_adapter.h

@@ -0,0 +1,43 @@
+#ifndef SIGHTSEEING_ADAPTER_H
+#define SIGHTSEEING_ADAPTER_H
+
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class SightseeingAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        SightseeingAdapter();
+                        ~SightseeingAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+                        int seizingCount=0;
+
+            };
+
+    }
+}
+
+
+#endif // HAPO_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/vv7_adapter.cpp

@@ -0,0 +1,150 @@
+#include <decition/adc_adapter/vv7_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+
+using namespace std;
+
+
+iv::decition::VV7Adapter::VV7Adapter(){
+    adapter_id= 2;
+    adapter_name="vv7";
+}
+iv::decition::VV7Adapter::~VV7Adapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::VV7Adapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+
+
+    float controlSpeed=accAim;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+    if (accAim < 0)
+    {
+        //0110
+        if(changingDangWei){
+            controlSpeed=min(-0.5f,controlSpeed);
+        }
+//        else if((obsDistance<0 ||obsDistance>50)&&(abs(dSpeed-realSpeed)<3)){
+//             controlSpeed=max(-0.2f,controlSpeed);
+//        }
+    }
+    else
+    {
+        controlSpeed=min(1.0f,controlSpeed);
+
+    }
+
+    //0227 10m nei xianzhi shache
+    if(obsDistance<10 &&obsDistance>0){
+        controlSpeed=min(controlSpeed,-0.8f);
+    }
+
+
+    if(DecideGps00::minDecelerate<0){
+        controlSpeed = min(DecideGps00::minDecelerate, controlSpeed);
+    }
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    (*decition)->accelerator=controlSpeed;
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-500.0,(*decition)->wheel_angle);
+     (*decition)->wheel_angle=min((float)500.0,(*decition)->wheel_angle);
+
+    if((*decition)->accelerator>=0){
+        (*decition)->torque= (*decition)->accelerator;
+        (*decition)->brake=0;
+    }else{
+        (*decition)->torque=0;
+        (*decition)->brake=0-(*decition)->accelerator;
+    }
+
+
+
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
+    return *decition;
+}
+
+
+
+float iv::decition::VV7Adapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else if(ttc>1.6){
+        BrakeIntMax = 5;
+    }else if(ttc>0.8){
+        BrakeIntMax = 8;
+    }else{
+        BrakeIntMax = 10;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(10.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::VV7Adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)5.0,controlSpeed);
+    controlSpeed=max((float)-7.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/vv7_adapter.h

@@ -0,0 +1,46 @@
+#ifndef VV7_ADAPTER_H
+#define VV7_ADAPTER_H
+
+
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include<decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class VV7Adapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+                        int seizingCount=0;
+
+                        VV7Adapter();
+                        ~VV7Adapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+
+            };
+
+    }
+}
+
+
+
+
+
+#endif // VV7_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/zhongche_adapter.cpp

@@ -0,0 +1,198 @@
+#include <decition/adc_adapter/zhongche_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+
+using namespace std;
+
+
+iv::decition::ZhongcheAdapter::ZhongcheAdapter(){
+    adapter_id= 3;
+    adapter_name="zhongche";
+}
+iv::decition::ZhongcheAdapter::~ZhongcheAdapter(){
+
+}
+
+
+
+iv::decition::Decition iv::decition::ZhongcheAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+
+
+    float controlSpeed=accAim;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+
+
+
+
+
+     (*decition)->grade=1;
+     (*decition)->mode=1;
+     (*decition)->speak=0;
+     (*decition)->handBrake=0;
+     (*decition)->bright=false;
+     (*decition)->engine=0;
+     (*decition)->brakeLight=0;
+
+    if(ServiceCarStatus.bocheMode){
+        (*decition)->dangWei=2;
+    }else{
+        (*decition)->dangWei=1;
+    }
+
+
+    if(ttc>10 && (obsDistance>10||obsDistance<0)){
+
+
+    float controlSpeed=0;
+
+
+        if(DecideGps00::minDecelerate<0){
+            (*decition)->torque = 0;
+            (*decition)->brake = max((0-DecideGps00::minDecelerate)*90, (*decition)->brake);
+             dSpeed=10.0;
+        }else{
+
+            if(dSpeed-realSpeed>2){
+
+                if(lastTorque==0 ){
+                     controlSpeed=40;
+                }else{
+                    controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+                }
+                if(controlSpeed>lastTorque){
+                    controlSpeed=lastTorque+4;
+                }else if(controlSpeed>lastTorque){
+                     controlSpeed=lastTorque-4;
+                }
+                (*decition)->torque=controlSpeed;
+            }else{
+              (*decition)->torque=realSpeed*8+10;
+            }
+           (*decition)->torque=max( (*decition)->torque,40.0f);
+             (*decition)->torque=min( (*decition)->torque,90.0f);
+            (*decition)->brake=0;
+        }
+
+
+        dSpeed=max(0.0f,dSpeed);
+        dSpeed=min(10.0f, dSpeed);
+        (*decition)->speed=(int)dSpeed*10+5;
+
+        (*decition)->torque=min((float)100.0,(*decition)->torque);
+        (*decition)->torque=max((float)0.0,(*decition)->torque);
+        (*decition)->brake=min((float)100.0,(*decition)->brake);
+        (*decition)->brake=max((float)0.0,(*decition)->brake);
+
+        lastTorque=(*decition)->torque;
+
+        (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+        (*decition)->wheel_angle=max((float)-550.0,(*decition)->wheel_angle);
+         (*decition)->wheel_angle=min((float)550.0,(*decition)->wheel_angle);
+        return *decition;
+    }
+
+
+
+    if (accAim < 0)
+    {
+        if(obsDistance<10 && obsDistance>0 &&ttc>3){
+            controlBrake = (int)(u * 40.0) + 1;  //(u * 14.0) + 1;
+        } else{
+            if(ttc<3){
+                controlBrake = (int)(u * 60.0) + 1;  //(u * 14.0) + 1;
+            }
+            else if(ttc<5){
+                controlBrake = (int)(u * 40.0) + 1;  //(u * 14.0) + 1;
+            }else {
+                controlBrake = (int)(u * 20.0) + 1;  //(u * 14.0) + 1;
+            }
+        }
+          (*decition)->brake = controlBrake;
+        (*decition)->torque=0;
+
+    }
+    else
+    {
+        controlBrake = 0;
+        (*decition)->brake=0;
+
+        if(lastTorque==0){
+             (*decition)->torque = u*(-30)+((0-u)-ServiceCarStatus.mfCalAcc)*1+
+                    now_gps_ins.ins_pitch_angle*10;
+        }else{
+             (*decition)->torque = lastTorque+((0-u)-ServiceCarStatus.mfCalAcc)*10;
+        }
+        if(realSpeed<10){
+             (*decition)->torque=min((float)40.0,(*decition)->torque);
+        }else if(realSpeed<30){
+             (*decition)->torque=min((float)60.0,(*decition)->torque);
+        }
+
+         (*decition)->torque=min((float)99.0,(*decition)->torque);
+         (*decition)->torque=max((float)0.0,(*decition)->torque);
+
+    }
+
+    //0227 10m nei xianzhi shache
+    if(obsDistance<10 &&obsDistance>0){
+        (*decition)->torque=0;
+        (*decition)->brake=max((float)30.0,(*decition)->brake);
+    }
+
+
+    if(DecideGps00().minDecelerate<0){
+        dSpeed=10;
+        (*decition)->torque = 0;
+        (*decition)->brake = min((0-DecideGps00().minDecelerate)*90, (*decition)->brake);
+    }
+
+
+    (*decition)->torque=min((float)100.0,(*decition)->torque);
+    (*decition)->torque=max((float)0.0,(*decition)->torque);
+    (*decition)->brake=min((float)100.0,(*decition)->brake);
+    (*decition)->brake=max((float)0.0,(*decition)->brake);
+
+    lastTorque=(*decition)->torque;
+
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-550.0,(*decition)->wheel_angle);
+     (*decition)->wheel_angle=min((float)550.0,(*decition)->wheel_angle);
+
+
+
+    dSpeed=max(0.0f,dSpeed);
+    dSpeed=min(10.0f, dSpeed);
+    (*decition)->speed=(int)dSpeed*10+5;
+
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
+    return *decition;
+}
+
+
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_adapter/zhongche_adapter.h

@@ -0,0 +1,44 @@
+#ifndef ZHONGCHE_ADAPTER_H
+#define ZHONGCHE_ADAPTER_H
+
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include<decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class ZhongcheAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+                        int seizingCount=0;
+
+                        ZhongcheAdapter();
+                        ~ZhongcheAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+
+            };
+
+    }
+}
+
+
+
+
+#endif // ZHONGCHE_ADAPTER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_controller/base_controller.cpp

@@ -0,0 +1,39 @@
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_controller/base_controller.h>
+
+
+
+
+iv::decition::BaseController::BaseController(){
+
+}
+iv::decition::BaseController::~BaseController(){
+
+}
+
+/**
+  * @brief iv::decition::BaseController::getControlDecition
+  * 基类BaseController的抽象方法 用于计算横向控制的方向盘和纵向控制的加速度
+  *
+  * @param now_gps_ins          实时gps信息
+  * @param path                 目标路径
+  * @param dSpeed               决策速度
+  * @param obsDistacne          障碍物距离
+  * @param obsSpeed             障碍物速度
+  * @param computeAngle         是否要计算方向盘角度
+  * @param computeAcc           是否要计算纵向加速度
+  * @param decition             决策信息结构体的指针
+  * @return                     iv::decition::Decition
+  */
+
+ iv::decition::Decition  iv::decition::BaseController::getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D> path, float dSpeed, float obsDistacne,
+                                                                          float obsSpeed,bool computeAngle, bool computeAcc,Decition *decition){
+
+}
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_controller/base_controller.h

@@ -0,0 +1,67 @@
+#pragma once
+#ifndef _IV_DECITION__BASE_CONTROLLER_
+#define _IV_DECITION__BASE_CONTROLLER_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+
+namespace iv {
+namespace decition {
+//根据传感器传输来的信息作出决策
+class BaseController {
+public:
+
+    int controller_id;
+    std::string  controller_name;
+
+    BaseController();
+    ~BaseController();
+
+
+    /**
+      * @brief iv::decition::BaseController::getControlDecition
+      * 基类BaseController的抽象方法 用于计算横向控制的方向盘和纵向控制的加速度
+      *
+      * @param now_gps_ins          实时gps信息
+      * @param path                 目标路径
+      * @param dSpeed               决策速度
+      * @param obsDistacne          障碍物距离
+      * @param obsSpeed             障碍物速度
+      * @param computeAngle         是否要计算方向盘角度
+      * @param computeAcc           是否要计算纵向加速度
+      * @param decition             决策信息结构体的指针
+      * @return                     iv::decition::Decition
+      */
+    virtual  iv::decition::Decition getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D>  path , float dSpeed, float obsDistacne , float  obsSpeed,
+                                                       bool computeAngle, bool computeAcc, Decition *decition);
+
+
+private:
+
+
+};
+
+}
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif // ADC_CONTROLLER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_controller/pid_controller.cpp

@@ -0,0 +1,336 @@
+#include <decition/adc_controller/pid_controller.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+
+using namespace std;
+
+
+iv::decition::PIDController::PIDController(){
+    controller_id = 0;
+    controller_name="pid";
+}
+iv::decition::PIDController::~PIDController(){
+
+}
+
+
+
+/**
+ * @brief getControlDecition
+ * pid方式计算横向方向盘和纵向加速度
+ *
+ * @param now_gps_ins          实时gps信息
+ * @param path                 目标路径
+ * @param dSpeed               决策速度
+ * @param ObsDistacne          障碍物距离
+ * @param ObsSpeed             障碍物速度
+ * @param computeAngle         是否要计算方向盘角度
+ * @param computeAcc           是否要计算纵向加速度
+ * @param decition             决策信息结构体的指针
+ * @return                     iv::decition::Decition
+ */
+
+iv::decition::Decition iv::decition::PIDController::getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D> path, float dSpeed, float obsDistacne, float obsSpeed,
+                                                                       bool computeAngle, bool computeAcc, Decition *decition){
+
+    float realSpeed= now_gps_ins.speed;
+    int roadMode = now_gps_ins.roadMode;
+    if(computeAngle){
+        (*decition)->wheel_angle=getPidAngle( realSpeed,  path,roadMode);
+    }
+    if(computeAcc){
+        (*decition)->accelerator=getPidAcc( now_gps_ins, dSpeed,  obsDistacne,  obsSpeed);
+    }
+    return *decition;
+}
+
+
+
+
+float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
+    if(ServiceCarStatus.msysparam.mvehtype=="zhongche"){
+        KEang = 14, KEPos = 100,IEpos=3,IEang=0.5;
+    }
+
+
+    bool turning=false;
+    if(roadMode==14||roadMode==15||roadMode==16||roadMode==17){
+        turning=true;
+    }
+
+    double PreviewDistance;//预瞄距离
+
+    if(ServiceCarStatus.msysparam.mvehtype=="ge3" || ServiceCarStatus.msysparam.mvehtype=="vv7"){
+        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
+    }else{
+        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+    }
+
+    //    if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+    //        KEang = 14, KEPos = 7, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
+    //        realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+    //    }
+    //   if(realSpeed <15){
+    //    PreviewDistance = max(4.0, realSpeed *0.4) ;
+    //   }
+    //    if(realSpeed <10){
+    //        PreviewDistance = max(3.0, realSpeed *0.3) ;
+    //    }
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+    int nsize = trace.size();
+    for (int i = 1; i < nsize-1; i++)
+    {
+        sumdis += GetDistance(trace[i - 1], trace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+    if(gpsIndex == 0)   gpsIndex = nsize -1;
+
+    EPos = trace[gpsIndex].x;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), trace.size()); i++) {
+        farTrace.push_back(trace[i]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAveDef(farTrace);
+    }
+    if(abs(realSpeed)<3){
+        eAngSum=0;
+        ePosSum=0;
+    }else{
+        eAngSum=eAngSum*0.8+EAng;
+        ePosSum=ePosSum*0.8+EPos;
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)
+            +IEang*eAngSum + IEpos*ePosSum;
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    //    if (ang > angleLimit) {
+    //        ang = angleLimit;
+    //    }
+    //    else if (ang < -angleLimit) {
+    //        ang = -angleLimit;
+    //    }
+
+        if (lastAng >-3000&&lastAng<3000) {
+            ang = 0.2 * lastAng + 0.8 * ang;
+        }
+
+    if(ang >-3000&&ang<3000){
+        lastAng = ang;
+    }else{
+        ang=lastAng;
+    }
+    return ang;
+
+}
+
+
+
+float iv::decition::PIDController::getAveDef(std::vector<Point2D> farTrace){
+    double sum_x = 0;
+    double sum_y = 0;
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+    return atan(average_x / average_y) / M_PI * 180;
+}
+
+
+
+float iv::decition::PIDController::getPidAcc (GPS_INS gpsIns,float dSpeed, float obsDistance, float obsSpeed){
+
+    std::cout << "\n计算用obs速度:%f\n" << obsSpeed << std::endl;
+    std::cout << "\n计算用obs距离:%f\n" << obsDistance << std::endl;
+    if(obsDistance<8 && obsDistance>0){
+        dSpeed=0;
+    }
+    float dSecSpeed = dSpeed / 3.6;
+    float realSpeed=gpsIns.speed;
+    float secSpeed =realSpeed/3.6;
+    double vt = dSecSpeed;
+    double vs = secSpeed;
+    if (abs(vs) < 0.05) vs = 0;
+    if (abs(obsSpeed) < 0.05) obsSpeed = 0;
+    double vl = vs + obsSpeed;
+    double vh = vs;
+    double dmax = 150;
+
+    //车距滤波
+    if (obsDistance < 0||obsDistance>100) {
+        obsDistance = 500;
+        obsSpeed=0;
+    }
+
+
+    if (obsDistance > 150) vl = 25; //25m/s
+
+
+    //TTC计算
+    double ds = 0.2566 * vl * vl + 5;
+    double ttcr = (vh - vl) / obsDistance;
+    double ttc = 15;
+    if (15 * (vh - vl) > obsDistance)
+        ttc = obsDistance / (vh - vl);
+
+
+ //   ServiceCarStatus.mfttc = ttc;
+
+
+    if (obsDistance <= dmax)
+    {
+        if (ttc > 10)
+        {
+            if (obsDistance > ds + 5)
+            {
+                double dds = min(30.0, obsDistance - (ds + 5));
+                vt = vt * dds / 30 + vl * (1 - dds / 30);
+            }
+            else
+            {
+                vt = min(vt, vl);
+            }
+        }
+        else
+        {
+            vt = min(vt, vl);
+        }
+    }else{
+        vt=dSecSpeed;
+    }
+
+    vt=min((float)vt,dSecSpeed);
+    std::cout << "\nvt:%f\n" << vt << std::endl;
+
+    //计算加速度
+    float u =computeU(obsDistance,ds,vs,vt,vh,vl,ttc,ttcr);
+
+    //u 限值
+    u=limiteU(u,ttc);
+
+    lastVt = vt;
+    lastU = u;
+    float acc=0-u;
+    return acc;
+}
+
+
+float iv::decition::PIDController::computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr){
+
+    double Id = 0;
+    double ed = ds - obsDistance;
+    if (obsDistance > 150) ed = 0;
+    double ev = vs - vt;
+    double u = 0;
+    if (ttc>10)
+    {
+        double kp = 0.5;        //double kp = 0.5;
+        double kd = 0.5;       //kd = 0.03
+        //      double k11 = 0.025;
+        //      double k12 = 0.000125;
+        double dev = (ev - lastEv) / 0.1;
+        Iv = 0.925 * Iv + ev;
+        Id = 0.85 * Id + ed;
+        if (abs(vh) < 2.0&& abs(vl) < 2.0)
+        {
+            Iv = 0.0; Id = 0.0;
+        }
+        //u = kp * ev + kd * dev + k11 * Iv + k12 * Id;
+        u = kp * ev + kd * dev;
+    }
+    else if (ttc < 1.6 || (ttc <= 10 && obsDistance <= 0.75 * ds))
+    {
+        //AEB控制
+        Id = 0; Iv = 0;
+        u = 0;
+        if (ttc < 0.8) u = 7;
+        else if (ttc < 1.6) u = 3 + 5 * (1.6 - ttc);
+        else
+        {
+            u = (10 - ttc) * (10 - ttc) / 23.52;
+        }
+    }
+    else
+    {
+        //制动控制
+        Id = 0; Iv = 0;
+        if (obsDistance > 1.25 * ds)
+        {
+            double rev_ed = 1 / ed;
+            u = -0.5 * obsDistance * obsDistance * rev_ed * ttcr * ttcr;
+        }
+        else
+        {
+            if (abs(vl) > 2.0)
+                u = 0;
+            else
+                u = max(lastU, 1.0f);
+        }
+    }
+    if (abs(vl) < 1.0 && abs(vh) < 1.0
+            && obsDistance < 2 * ds)
+    {
+        u = 0.5;
+    }
+    lastEv = ev;
+    return u;
+}
+
+
+float iv::decition::PIDController::limiteU(float u,float ttc){
+    if(ttc<3 && u<-0.2){
+        u=-0.2;
+    }
+    else
+    {
+        if (u < -1.5) u = -1.5;
+    }
+    if (u >= 0 && lastU <= 0)
+    {
+        if (u > 0.5) u = 0.5;
+    }
+    else if (u >= 0 && lastU >= 0)
+    {
+        if (u > lastU + 0.5) u = lastU + 0.5;
+    }
+    else if (u <= 0 && lastU >= 0)
+    {
+        if (u < -0.04) u = -0.04;
+    }
+    else if (u <= 0 && lastU <= 0)
+    {
+        if (u < lastU - 0.04) u = lastU - 0.04;
+    }
+    return u;
+}
+
+
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_controller/pid_controller.h

@@ -0,0 +1,76 @@
+#ifndef _IV_DECITION__PID_CONTROLLER_
+#define _IV_DECITION__PID_CONTROLLER_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_controller/base_controller.h>
+#include <decition/adc_tools/transfer.h>
+
+namespace iv {
+        namespace decition {
+        //根据传感器传输来的信息作出决策
+                    class PIDController: public BaseController {
+                    public:
+
+                        float lastEA;
+                        float lastEP;
+                        float lastAng;
+                        float angleLimit=600;
+                        float lastU ;
+                        float lastEv ;
+                        float lastVt ;
+
+                        float Iv;
+                        float eAngSum=0;
+                        float ePosSum=0;
+
+                        PIDController();
+                        ~PIDController();
+
+                        /**
+                           * @brief getControlDecition
+                           * pid方式计算横向方向盘和纵向加速度
+                           *
+                           * @param now_gps_ins          实时gps信息
+                           * @param path                 目标路径
+                           * @param dSpeed               决策速度
+                           * @param obsDistacne          障碍物距离
+                           * @param obsSpeed             障碍物速度
+                           * @param computeAngle         是否要计算方向盘角度
+                           * @param computeAcc           是否要计算纵向加速度
+                           * @param decition             决策信息结构体的指针
+                           * @return                     iv::decition::Decition
+                           */
+
+
+                        iv::decition::Decition getControlDecition(GPS_INS now_gps_ins, std::vector<Point2D> path,float dSpeed, float obsDistacne,float  obsSpeed,
+                                                                  bool computeAngle, bool computeAcc, Decition *decition);
+                        float  getPidAngle (float realSpeed, std::vector<Point2D> trace,int roadMode);
+                        float  getAveDef (std::vector<Point2D> farTrace);
+                        float  getPidAcc (GPS_INS gpsIns,float dSpeed, float obsDistacne, float obsSpeed);
+                        float  computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr);
+                        float  limiteU(float u ,float ttc);
+
+
+                    private:
+
+                    };
+
+        }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+#endif // PID_CONTROLLER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/base_planner.cpp

@@ -0,0 +1,40 @@
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_tools/transfer.h>
+#include "perception/perceptionoutput.h"
+#include <decition/adc_planner/base_planner.h>
+
+
+
+
+
+
+iv::decition::BasePlanner::BasePlanner(){
+
+}
+iv::decition::BasePlanner::~BasePlanner(){
+
+}
+
+
+/**
+ * @brief iv::decition::BasePlanner::getPath
+ * 基类BasePlanner的抽象方法，用于计算车辆行驶的路径。路径都已转换成车辆坐标系下的路径。
+ *
+ * @param now_gps_ins          实时gps信息
+ * @param gpsMapLine           地图数据点
+ * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+ * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
+ * @param lidarGridPtr         激光雷达信息网格
+ * @return                     返回一条车辆坐标系下的路径
+ */
+std::vector<iv::Point2D> iv::decition::BasePlanner::getPath(GPS_INS now_gps_ins,const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
+
+}
+
+//std::vector<iv::Point2D> iv::decition::BasePlanner::getPath (GPS_INS now_gps_ins,  std::vector<GPSData> gpsMapLine,  LidarGridPtr lidarGridPtr, std::vector<iv::Perception::PerceptionOutput> lidar_per){
+
+//}

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/base_planner.h

@@ -0,0 +1,48 @@
+#ifndef _IV_DECITION__BASE_PLANNER_
+#define _IV_DECITION__BASE_PLANNER_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include "perception/perceptionoutput.h"
+
+namespace iv {
+namespace decition {
+//根据传感器传输来的信息作出决策
+    class BasePlanner {
+    public:
+        int planner_id;
+        std::string  planner_name;
+
+        BasePlanner();
+        virtual ~BasePlanner();
+
+        /**
+         * @brief iv::decition::BasePlanner::getPath
+         * 基类BasePlanner的抽象方法，用于计算车辆行驶的路径。路径都已转换成车辆坐标系下的路径。
+         *
+         * @param now_gps_ins          实时gps信息
+         * @param gpsMapLine           地图数据点
+         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
+         * @param lidarGridPtr         激光雷达信息网格
+         * @return                     返回一条车辆坐标系下的路径
+         */
+        virtual  std::vector<iv::Point2D> getPath (GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
+//        virtual  std::vector<iv::Point2D> getPath (GPS_INS now_gps_ins,  std::vector<GPSData> gpsMapLine,  LidarGridPtr lidarGridPtr, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+
+    private:
+
+    };
+
+}
+}
+
+
+#endif // ADC_PLANNER_H
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/dubins_planner.cpp

@@ -0,0 +1,57 @@
+#include <decition/adc_planner/dubins_planner.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/adc_tools/parameter_status.h>
+#include <common/constants.h>
+
+#include <math.h>
+
+std::vector<iv::GPSData> iv::decition::DubinsPlanner::gpsMap;
+
+iv::decition::DubinsPlanner::DubinsPlanner(){
+    this->planner_id = 2;
+    this->planner_name = "Dubins";
+}
+
+iv::decition::DubinsPlanner::~DubinsPlanner(){
+}
+
+/**
+ * @brief iv::decition::LaneChangePlanner::getPath
+ * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
+ *
+ * @param now_gps_ins          实时gps信息
+ * @param gpsMapLine           地图数据点
+ * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+ * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
+ * @param lidarGridPtr         激光雷达信息网格
+ * @return                     返回一条车辆坐标系下的路径
+ */
+std::vector<iv::Point2D> iv::decition::DubinsPlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
+    std::vector<iv::Point2D> trace;
+
+    double L = 2.6;
+    double R = L/sin(10.0*M_PI/180.0);
+    std::cout<<" R = "<<R<<std::endl;
+//    return a.exec();
+//    double q0[] = { 0,0,1.57};
+//    double q1[] = { 4,4,3.142 };
+    gpsMap.clear();
+    double q0[] = { now_gps_ins.gps_x, now_gps_ins.gps_y, now_gps_ins.ins_heading_angle};
+    double q1[] = { gpsMapLine[PathPoint]->gps_x, gpsMapLine[PathPoint]->gps_y ,gpsMapLine[PathPoint]->ins_heading_angle };
+    double turning_radius = 10.0;
+    DubinsPath path;
+    dubins_shortest_path( &path, q0, q1, turning_radius);
+    dubins_path_sample_many( &path, 0.1,  printConfiguration, NULL);
+
+
+}
+
+int  iv::decition::DubinsPlanner::printConfiguration(double q[3], double x, void* user_data) {
+    GPSData gps;
+    gps->roadMode=5;
+    gps->gps_x=q[0];
+    gps->gps_y=q[1];
+    gps->ins_heading_angle=q[3];
+    gpsMap.push_back(gps);
+    return 0;
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/dubins_planner.h

@@ -0,0 +1,44 @@
+#ifndef DUBINS_PLANNER_H
+#define DUBINS_PLANNER_H
+#include "base_planner.h"
+#include <decition/adc_tools/dubins.h>
+#include <QCoreApplication>
+
+#include <math.h>
+#include <iostream>
+
+
+
+#include <stdio.h>
+#include <string.h>
+
+namespace iv{
+namespace decition{
+    class DubinsPlanner : public BasePlanner{
+    public:
+        DubinsPlanner();
+        ~DubinsPlanner();
+
+         static std::vector<iv::GPSData>  gpsMap;
+
+        /**
+         * @brief iv::decition::LaneChangePlanner::getPath
+         * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
+         *
+         * @param now_gps_ins          实时gps信息
+         * @param gpsMapLine           地图数据点
+         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
+         * @param lidarGridPtr         激光雷达信息网格
+         * @return                     返回一条车辆坐标系下的路径
+         */
+        std::vector<iv::Point2D> getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
+
+        int  static printConfiguration(double q[3], double x, void* user_data);
+
+        };
+    }
+}
+
+
+#endif // DUBINS_PLANNER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/frenet_planner.cpp

@@ -0,0 +1,643 @@
+#include "frenet_planner.h"
+
+#include <decition/adc_tools/transfer.h>
+#include <common/car_status.h>
+#include <decition/adc_tools/parameter_status.h>
+#include <decition/decide_gps_00.h>
+#include <decition/adc_planner/lane_change_planner.h>
+
+#include<Eigen/Dense>
+
+using namespace std;
+using namespace Eigen;
+
+iv::decition::FrenetPlanner::FrenetPlanner(){
+    this->planner_id = 1;
+    this->planner_name = "Frenet";
+    this->lidarGridPtr = NULL;
+}
+
+iv::decition::FrenetPlanner::~FrenetPlanner(){
+    free(this->lidarGridPtr);
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::getPath
+ * 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。
+ * 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。
+ *
+ * @param now_gps_ins          实时gps信息
+ * @param gpsMapLine           地图数据点
+ * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+ * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。offset = -(roadNow - roadOri)*roadWidth
+ * @param lidarGridPtr         激光雷达信息网格
+ * @return                     返回一条车辆坐标系下的路径
+ */
+std::vector<iv::Point2D> iv::decition::FrenetPlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
+    this->roadNow = -1;
+    this->now_gps_ins = now_gps_ins;
+    this->gpsMapLine = gpsMapLine;
+    this->PathPoint = PathPoint;
+    this->lidarGridPtr = lidarGridPtr;
+    LaneChangePlanner *lcp = new LaneChangePlanner();
+    std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+    delete lcp;
+    double realSecSpeed = now_gps_ins.speed / 3.6;
+    leftWidth = offset;
+    rightWidth = offset;
+    std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
+    return trace;
+}
+
+/**
+ * @brief iv::decition::FrenetPlanner::chooseRoadByFrenet
+ * 用frenet的方法对每条道路考察，选择一个最优的道路
+ * @param now_gps_ins          实时gps信息
+ * @param gpsMapLine           地图数据点
+ * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+ * @param offsetL              在选择道路中，地图源路线左边的最大宽度。可以为负值。offsetL = -(roadSum - 1)*roadWidth
+ * @param offsetR              在选择道路中，地图源路线右边的最大宽度。可以为非负值。offsetR = (roadOri - 0)*roadWidth
+ * @param lidarGridPtr         激光雷达信息网格
+ * @return
+ */
+int iv::decition::FrenetPlanner::chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow){
+    this->roadNow = roadNow;
+    this->aimRoadByFrenet = -1;
+    this->now_gps_ins = now_gps_ins;
+    this->gpsMapLine = gpsMapLine;
+    this->PathPoint = PathPoint;
+    this->lidarGridPtr = lidarGridPtr;
+
+    LaneChangePlanner *lcp = new LaneChangePlanner();
+    std::vector<iv::Point2D> gpsTrace = lcp->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+    delete lcp;
+    double realSecSpeed = now_gps_ins.speed / 3.6;
+    leftWidth = offsetL;
+    rightWidth = offsetR;
+    std::vector<iv::Point2D> trace = getGpsTrace_AfterCalcFrenetTrace(gpsTrace,realSecSpeed);
+    return aimRoadByFrenet;
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace
+ * 进行frenet规划的总函数，还包括了规划前的一些准备工作，比如frenet坐标系建立、车辆起始状态等。
+ * @param gpsTrace             地图路线上从PathPoint开始的600个点
+ * @param realsecSpeed         实时车速 [m/s]
+ * @return                     返回一条frenet规划后并转换到车辆坐标系下的路径
+ */
+std::vector<iv::Point2D> iv::decition::FrenetPlanner::getGpsTrace_AfterCalcFrenetTrace(const std::vector<iv::Point2D>& gpsTrace,double realsecSpeed){
+    vector<Point2D> trace;
+    vector<FrenetPoint> frenet_s;
+    double stp=0.0;
+    //把gpsTrace里的点全部转为frenet坐标系下的点，存储在frenet_s中。相当于frenet坐标系的S轴。
+    FrenetPoint tmp={.x=gpsTrace[0].x,.y=gpsTrace[0].y,.s=0.0,.d=0.0};
+    frenet_s.push_back(tmp);
+    for(int i=1; i<gpsTrace.size(); ++i){
+        stp+=iv::decition::GetDistance(gpsTrace[i-1],gpsTrace[i]);
+        tmp={.x=gpsTrace[i].x,.y=gpsTrace[i].y,.s=stp,.d=0.0};
+        frenet_s.push_back(tmp);
+    }
+
+    //求出车辆当前位置在frenet坐标系下的坐标
+//    FrenetPoint car_now_at_frenet = XY2Frenet(0,0,gpsTrace);
+    FrenetPoint car_now_at_frenet = getFrenetfromXY(0,0,gpsTrace,this->gpsMapLine,this->PathPoint,this->now_gps_ins);
+
+    double c_s_speed = realsecSpeed * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
+    double c_d_speed = realsecSpeed * sin(car_now_at_frenet.tangent_Ang-M_PI/2);
+    double realAcc = ServiceCarStatus.mfCalAcc;
+    double c_s_dd = realAcc * cos(car_now_at_frenet.tangent_Ang-M_PI/2);
+    double c_d_dd = realAcc * sin(car_now_at_frenet.tangent_Ang-M_PI/2);
+
+    vector<Point2D> path=frenet_optimal_planning(car_now_at_frenet,frenet_s,c_s_speed,c_d_speed,c_d_dd);
+
+    return path;
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::frenet_optimal_planning
+ * 对frenet规划出的路径进行后续操作，并选出损失最小的一条路径作为最优路径
+ * @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
+ * @param frenet_s             frenet坐标系的s轴
+ * @param c_speed              车辆的纵向速度。沿s轴方向的速度。
+ * @param c_d_d                车辆沿d方向的速度。
+ * @param c_d_dd               车辆沿d方向的加速度。
+ * @return                     返回一条frenet规划的最优路径
+ */
+vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_optimal_planning(iv::decition::FrenetPoint car_now_frenet_point,
+                                                          const std::vector<FrenetPoint>& frenet_s, double c_speed, double c_d_d, double c_d_dd){
+    double s0 = car_now_frenet_point.s;
+    double c_d = car_now_frenet_point.d;
+    vector<iv::Point2D> gpsTrace;
+    vector<Frenet_path> fplist=calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0);  //frenet规划
+
+    sort(fplist.begin(),fplist.end(),comp); //按损失度由小到大进行排序
+    for(int i=0; i<fplist.size(); ++i){
+        calc_global_single_path(fplist[i],frenet_s);
+        if(check_single_path(fplist[i])){
+            gpsTrace = frenet_path_to_gpsTrace(fplist[i]);
+            aimRoadByFrenet = fplist[i].roadflag;
+            return gpsTrace;
+        }
+    }
+    return gpsTrace;
+
+/*
+//    calc_global_paths(fplist, frenet_s);    //生成frenet路径中每个点在车辆坐标系下的坐标
+//    fplist = check_paths(fplist);  //检验计算出的每条路径
+//    //找到损失最小的轨迹
+//    double min_cost = (numeric_limits<double>::max)();
+//    Frenet_path bestpath;
+//    for(int i=0; i<fplist.size(); ++i){
+//        if(min_cost > fplist[i].cf){
+//            min_cost = fplist[i].cf;
+//            bestpath = fplist[i];
+//        }
+//    }
+//    gpsTrace = frenet_path_to_gpsTrace(bestpath);
+//    aimRoadByFrenet = bestpath.roadflag;
+//    return gpsTrace;
+*/
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::calc_frenet_paths
+ * 正式规划出不同的frenet路径，并统计相应的损失度。单从车辆行驶的平滑性计算而得，后续需检测是否有障碍物。
+ * @param c_speed              车辆的纵向速度。沿s轴方向的速度
+ * @param c_d                  车辆的横向（d方向）偏移距离
+ * @param c_d_d                车辆沿d方向的速度
+ * @param c_d_dd               车辆沿d方向的加速度
+ * @param s0                   车辆沿s方向的坐标
+ * @return                     返回多条frenet路径
+ */
+vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0){
+    vector<iv::decition::Frenet_path> frenet_paths;
+    int roadNum = round(abs((leftWidth-rightWidth)/ServiceParameterStatus.D_ROAD_W));   //roadNum为frenet算法的起始道路序号
+    //采样，并对每一个目标配置生成轨迹
+    for(double di=leftWidth; di<=rightWidth; di+=ServiceParameterStatus.D_ROAD_W,roadNum--){
+        if(roadNum == this->roadNow)
+            continue;
+        //横向动作规划
+        for(double Ti=ServiceParameterStatus.MINT; Ti<ServiceParameterStatus.MAXT; Ti+=ServiceParameterStatus.DT){
+            Frenet_path fp;
+            fp.roadflag = roadNum;
+            //计算出关于目标配置di，Ti的横向多项式
+            Quintic_polynomial lat_qp = Quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti);
+
+            for(double i=0; i<Ti; i+=ServiceParameterStatus.D_POINT_T)
+                fp.t.push_back(i);
+
+            for(int i=0; i<fp.t.size(); ++i){
+                fp.d.push_back(lat_qp.calc_point(fp.t[i]));
+                fp.d_d.push_back(lat_qp.calc_first_derivative(fp.t[i]));
+                fp.d_dd.push_back(lat_qp.calc_second_derivative(fp.t[i]));
+                fp.d_ddd.push_back(lat_qp.calc_third_derivative(fp.t[i]));
+            }
+
+            //纵向速度规划 (速度保持)
+            for(double tv=ServiceParameterStatus.TARGET_SPEED - ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE;
+                tv<(ServiceParameterStatus.TARGET_SPEED + ServiceParameterStatus.D_T_S * ServiceParameterStatus.N_S_SAMPLE);
+                tv+=ServiceParameterStatus.D_T_S){
+                Frenet_path tfp = fp;
+                Quartic_polynomial lon_qp = Quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti);
+
+                for(int i=0; i<fp.t.size(); ++i){
+                    tfp.s.push_back(lon_qp.calc_point(fp.t[i]));
+                    tfp.s_d.push_back(lon_qp.calc_first_derivative(fp.t[i]));
+                    tfp.s_dd.push_back(lon_qp.calc_second_derivative(fp.t[i]));
+                    tfp.s_ddd.push_back(lon_qp.calc_third_derivative(fp.t[i]));
+                }
+
+                //square of jerk
+                double Jp = inner_product(tfp.d_ddd.begin(), tfp.d_ddd.end(), tfp.d_ddd.begin(), 0);
+                //square of jerk
+                double Js = inner_product(tfp.s_ddd.begin(), tfp.s_ddd.end(), tfp.s_ddd.begin(), 0);
+
+                //square of diff from target speed
+                double ds = pow((ServiceParameterStatus.TARGET_SPEED - tfp.s_d.back()),2);
+                //横向的损失函数
+                tfp.cd = ServiceParameterStatus.KJ * Jp + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * tfp.d.back() * tfp.d.back();
+                //纵向的损失函数
+                tfp.cv = ServiceParameterStatus.KJ * Js + ServiceParameterStatus.KT * Ti + ServiceParameterStatus.KD * ds;
+                //总的损失函数为d 和 s方向的损失函数乘对应的系数相加
+                tfp.cf = ServiceParameterStatus.KLAT * tfp.cd + ServiceParameterStatus.KLON * tfp.cv;
+
+                frenet_paths.push_back(tfp);
+            }
+        }
+    }
+    return frenet_paths;
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::calc_global_paths
+ * 计算出每条frenet路径中轨迹点的x，y坐标。x，y坐标是轨迹点在车辆坐标系下的横纵坐标。
+ * 部分函数参数在第一种计算XY坐标的方法中没有使用，但在第二种方法中会用到。
+ * @param fplist               多条frenet路径
+ * @param frenet_s             frenet坐标系的s轴
+ * @return                     通过引用传递返回带有x，y值的多条frenet路径
+ */
+void iv::decition::FrenetPlanner::calc_global_paths(vector<Frenet_path>& fplist,const std::vector<FrenetPoint>& frenet_s){
+
+    for(auto& fp:fplist){
+        for(int i=0; i<fp.s.size(); ++i){
+            double ix,iy;
+//            Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s);  //第一种方法
+            getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins);   //第二种方法
+
+            fp.x.push_back(ix);
+            fp.y.push_back(iy);
+        }
+
+        for(int i=0; i<(fp.x.size()-1); ++i){
+            double dx = fp.x[i+1] - fp.x[i];
+            double dy = fp.y[i+1] - fp.y[i];
+            fp.yaw.push_back(atan2(dy,dx));
+            fp.ds.push_back(sqrt(dx*dx + dy*dy));
+        }
+
+        fp.ds.push_back(fp.ds.back());
+        fp.yaw.push_back(fp.yaw.back());
+
+        for(int i = 0; i < (fp.yaw.size() - 1); ++i){
+            fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
+        }
+    }
+}
+
+void iv::decition::FrenetPlanner::calc_global_single_path(Frenet_path &fp, const std::vector<FrenetPoint> &frenet_s){
+    for(int i=0; i<fp.s.size(); ++i){
+        double ix,iy;
+        //            Frenet2XY(&ix,&iy,fp.s[i],fp.d[i],frenet_s);  //第一种方法
+        getXYfromFrenet(&ix,&iy,fp.s[i],fp.d[i],frenet_s,this->gpsMapLine,this->PathPoint,this->now_gps_ins);   //第二种方法
+
+        fp.x.push_back(ix);
+        fp.y.push_back(iy);
+    }
+
+    for(int i=0; i<(fp.x.size()-1); ++i){
+        double dx = fp.x[i+1] - fp.x[i];
+        double dy = fp.y[i+1] - fp.y[i];
+        fp.yaw.push_back(atan2(dy,dx));
+        fp.ds.push_back(sqrt(dx*dx + dy*dy));
+    }
+
+    fp.ds.push_back(fp.ds.back());
+    fp.yaw.push_back(fp.yaw.back());
+
+    for(int i = 0; i < (fp.yaw.size() - 1); ++i){
+        fp.c.push_back((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]);
+    }
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::check_paths
+ * 对多条frenet路径进行检验，排除不符合要求的路径。
+ * @param fplist               多条frenet路径
+ * @return                     排除部分路径后的多条frenet路径
+ */
+vector<iv::decition::Frenet_path> iv::decition::FrenetPlanner::check_paths(const vector<Frenet_path>& fplist){
+    vector<Frenet_path> okind;
+    int i=0;
+    int j=0;
+
+    for(i=0; i<fplist.size(); ++i){
+        cout<<"&&&&&&&&&&fplist[i].roadflag: "<<fplist[i].roadflag<<endl;
+        for(j=0; j<fplist[i].s_d.size(); ++j){
+            if(fplist[i].s_d[j]>ServiceParameterStatus.MAX_SPEED)       //最大速度检查
+                goto ContinueFlag;
+        }
+        for(j=0; j<fplist[i].s_dd.size(); ++j){
+            if(abs(fplist[i].s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
+                goto ContinueFlag;
+        }
+        for(j=30; j<fplist[i].c.size()-30; ++j){
+            if(abs(fplist[i].c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
+                goto ContinueFlag;
+        }
+        if(!check_collision(fplist[i]))
+            continue;
+        okind.push_back(fplist[i]);
+        ContinueFlag:continue;
+    }
+    return okind;
+}
+
+
+bool iv::decition::FrenetPlanner::check_single_path(const iv::decition::Frenet_path &fp){
+    int j=0;
+
+    cout<<"&&&&&&&&&&fp.roadflag: "<<fp.roadflag<<endl;
+    for(j=0; j<fp.s_d.size(); ++j){
+        if(fp.s_d[j]>ServiceParameterStatus.MAX_SPEED)       //最大速度检查
+            return false;
+    }
+    for(j=0; j<fp.s_dd.size(); ++j){
+        if(abs(fp.s_dd[j])>ServiceParameterStatus.MAX_ACCEL) //最大加速度检查
+            return false;
+    }
+    for(j=30; j<fp.c.size()-30; ++j){
+        if(abs(fp.c[j])>ServiceParameterStatus.MAX_CURVATURE)//最大曲率检查
+            return false;
+    }
+    if(!check_collision(fp))
+        return false;
+
+    return true;
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::check_collision
+ * 检验frenet路径上是否有障碍物，调用的是改进的函数computeObsOnRoadByFrenet。
+ * @param frenet_path          一条frenet路径
+ * @param car_now_frenet_point 车辆当前位置在frenet坐标系下的坐标
+ * @return                     在路径上有障碍物返回false，否则返回true。
+ */
+bool iv::decition::FrenetPlanner::check_collision(const iv::decition::Frenet_path &frenet_path){
+    std::vector<Point2D> gpsTrace = frenet_path_to_gpsTrace(frenet_path);
+    double obs=-1;
+    iv::decition::DecideGps00().computeObsOnRoadByFrenet(this->lidarGridPtr,gpsTrace,obs,this->gpsMapLine,this->PathPoint,this->now_gps_ins);
+    if(obs > 0 && obs < 30)
+        return false;
+    else
+        return true;
+}
+
+
+/**
+ * @brief iv::decition::FrenetPlanner::frenet_path_to_gpsTrace
+ * 将frenet路径转换到车辆坐标系下。
+ * @param frenet_path          一条frenet路径
+ * @return                     一条车辆坐标系下的路径
+ */
+vector<iv::Point2D> iv::decition::FrenetPlanner::frenet_path_to_gpsTrace(const Frenet_path& frenet_path){
+    vector<iv::Point2D> gpsTrace;
+    for (int i=0; i<frenet_path.x.size(); ++i) {
+        gpsTrace.push_back(Point2D(frenet_path.x[i],frenet_path.y[i]));
+        gpsTrace[i].speed = sqrt(frenet_path.d_d[i]*frenet_path.d_d[i]+frenet_path.s_d[i]*frenet_path.s_d[i])*3.6;
+    }
+    return gpsTrace;
+}
+
+
+double iv::decition::FrenetPlanner::distance(double x1, double y1, double x2, double y2)
+{
+    return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
+}
+
+
+//找出gpsTrace中距离（x，y）最近的一个点
+int iv::decition::FrenetPlanner::ClosestWaypoint(double x, double y, const std::vector<Point2D>& gpsTrace)
+{
+
+    double closestLen = 100000; //large number
+    int closestWaypoint = 0;
+
+    for(int i = 1; i < gpsTrace.size(); i++)
+    {
+        double map_x = gpsTrace[i].x;
+        double map_y = gpsTrace[i].y;
+        double dist = distance(x,y,map_x,map_y);
+        if(dist < closestLen)
+        {
+            closestLen = dist;
+            closestWaypoint = i;
+        }
+
+    }
+    return closestWaypoint;
+}
+
+
+ /* |==========================================================|
+  * |         车辆坐标系与 frenet 坐标系互转的第一种方法。           |
+  * |==========================================================| */
+ // 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
+ // Transform from Cartesian x,y coordinates to Frenet s,d coordinates
+iv::decition::FrenetPoint iv::decition::FrenetPlanner::XY2Frenet(double x, double y, const std::vector<iv::Point2D>& gpsTrace){
+    int next_wp=ClosestWaypoint( x,  y,  gpsTrace);
+    int prev_wp = next_wp-1;  //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
+//    if(next_wp == 0){
+//        prev_wp  = gpsTrace.size()-1;
+//    }
+
+    double n_x = gpsTrace[next_wp].x-gpsTrace[prev_wp].x;
+    double n_y = gpsTrace[next_wp].y-gpsTrace[prev_wp].y;
+    double x_x = x - gpsTrace[prev_wp].x;
+    double x_y = y - gpsTrace[prev_wp].y;
+
+    // find the projection of x onto n
+    double proj_norm = (x_x*n_x+x_y*n_y)/(n_x*n_x+n_y*n_y);
+    double proj_x = proj_norm*n_x;
+    double proj_y = proj_norm*n_y;  //proj_x、proj_y应该为垂足的坐标
+
+    double frenet_d = sqrt((x_x - proj_x) * (x_x - proj_x) + (x_y - proj_y) * (x_y - proj_y));
+    //经手动推算frenet_d = abs(n_x * x_y - n_y * x_x) / sqrt(n_x * n_x + n_y * n_y)。
+    //即frenet_d，等于点(x,y)到 过prev_wp、next_wp两点的直线 的垂直距离。
+
+    //see if d value is positive or negative by comparing it to a center point
+    double center_x = 1000-gpsTrace[prev_wp].x;
+    double center_y = 2000-gpsTrace[prev_wp].y;
+    double centerToPos = distance(center_x,center_y,x_x,x_y);
+    double centerToRef = distance(center_x,center_y,proj_x,proj_y);
+    if(centerToPos <= centerToRef){
+        frenet_d *= -1;
+    }
+
+    // calculate s value
+    double frenet_s = 0;
+    for(int i = 0; i < prev_wp; i++){
+        frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
+    }
+    if(prev_wp<=0){
+        frenet_s -= distance(0,0,proj_x,proj_y);
+    }else{
+        frenet_s += distance(0,0,proj_x,proj_y);
+    }
+
+    double tmp_Angle = atan2(n_y,n_x);
+
+    return {x:x,y:y,s:frenet_s,d:frenet_d,tangent_Ang:tmp_Angle};
+}
+
+ // frenet坐标转车体坐标。
+ // Transform from Frenet s,d coordinates to Cartesian x,y
+void iv::decition::FrenetPlanner::Frenet2XY(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace){
+    int prev_wp = 0;
+    //退出循环时，prev_wp 最大是等于 frenetTrace.size()-2。
+    while(s > frenetTrace[prev_wp+1].s && (prev_wp < (int)(frenetTrace.size()-2) )){
+        prev_wp++;
+    }
+    int wp2 = prev_wp+1;
+    double heading = atan2((frenetTrace[wp2].y-frenetTrace[prev_wp].y),(frenetTrace[wp2].x-frenetTrace[prev_wp].x));
+
+    double seg_s = (s-frenetTrace[prev_wp].s);
+    double seg_x = frenetTrace[prev_wp].x+seg_s*cos(heading);
+    double seg_y = frenetTrace[prev_wp].y+seg_s*sin(heading);
+
+    double perp_heading = heading-M_PI*0.5;
+    *res_x = seg_x + d*cos(perp_heading);
+    *res_y = seg_y + d*sin(perp_heading);
+}
+
+/* |==========================================================|
+ * |         车辆坐标系与 frenet 坐标系互转的第二种方法。           |
+ * |==========================================================| */
+// 车体坐标转frenet坐标。frenet坐标原点是 gpsTrace 中序号为0的点。
+iv::decition::FrenetPoint iv::decition::FrenetPlanner::getFrenetfromXY(double x, double y, const std::vector<iv::Point2D>& gpsTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps){
+    int next_wp=ClosestWaypoint( x,  y,  gpsTrace);
+    int prev_wp = next_wp-1;  //prev_wp代表在next_wp的前一个点计算坐标，能够保证frenet_s都是依次相加的。避免在next_wp计算frenet_s的话，可能出现减的情况。
+    //    if(next_wp == 0){
+    //        prev_wp  = gpsTrace.size()-1;
+    //    }
+
+    GPS_INS gps = Coordinate_UnTransfer(x,y,nowGps);
+    Point2D pt = Coordinate_Transfer(gps.gps_x,gps.gps_y, *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);
+
+    // calculate s value
+    double frenet_s = 0;
+    for(int i = 0; i < prev_wp; i++){
+        frenet_s += distance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
+    }
+    frenet_s += pt.y;
+
+    //theta为prev_wp处车的航向角 与 nowGps处的车辆坐标系下x轴之间的夹角，逆时针为正。
+    double theta = (nowGps.ins_heading_angle - gpsMap[(pathpoint+prev_wp)%gpsMap.size()]->ins_heading_angle+90);
+    double tmp_Angle = theta * M_PI / 180;
+
+    return {x:x,y:y,s:frenet_s,d:pt.x,tangent_Ang:tmp_Angle};
+}
+
+void iv::decition::FrenetPlanner::getXYfromFrenet(double *res_x, double *res_y, double s, double d, const vector<iv::decition::FrenetPoint>& frenetTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
+{
+    int prev_wp = -1;
+
+    while((prev_wp < (int)(frenetTrace.size()-1) ) && s > frenetTrace[prev_wp+1].s)
+    {
+        prev_wp++;
+    }
+    if(prev_wp < 0){
+        prev_wp = 0;
+    }
+//    int wp2 =prev_wp+1;
+    GPS_INS gps=  Coordinate_UnTransfer( d, s-frenetTrace[prev_wp].s , *gpsMap[(pathpoint+prev_wp)%gpsMap.size()]);
+    Point2D pt =  Coordinate_Transfer( gps.gps_x, gps.gps_y, nowGps);
+
+    *res_x = pt.x;
+    *res_y = pt.y;
+}
+
+
+iv::decition::Quintic_polynomial::Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T){
+    //计算五次多项式系数
+    this->xs = xs;
+    this->vxs = vxs;
+    this->axs = axs;
+    this->xe = xe;
+    this->vxe = vxe;
+    this->axe = axe;
+
+    this->a0 = xs;
+    this->a1 = vxs;
+    this->a2 = axs/ 2.0;
+
+    MatrixXd A(3, 3);
+    MatrixXd b(3, 1);
+    MatrixXd x(3, 1);
+
+    A << pow(T, 3),   pow(T, 4),   pow(T, 5),
+         3*pow(T, 2), 4*pow(T, 3), 5*pow(T, 4),
+         6*T,         12*T*T,      20*pow(T, 3);
+    b << xe - a0 - a1*T - a2*T*T,
+         vxe - a1 - 2*a2*T,
+         axe - 2*a2;
+    x=A.colPivHouseholderQr().solve(b);
+
+    this->a3 = x(0,0);
+    this->a4 = x(1,0);
+    this->a5 = x(2,0);
+}
+
+iv::decition::Quintic_polynomial::~Quintic_polynomial(){
+}
+
+double iv::decition::Quintic_polynomial::calc_point(double t){
+    double xt = this->a0 + this->a1 * t + this->a2 * t*t + this->a3 * t*t*t + this->a4 * t*t*t*t + this->a5 * t*t*t*t*t;
+    return xt;
+}
+
+double iv::decition::Quintic_polynomial::calc_first_derivative(double t){
+    double xt = this->a1 + 2*this->a2 * t + 3*this->a3 * t*t + 4*this->a4 * t*t*t + 5*this->a5 * t*t*t*t;
+    return xt;
+}
+
+double iv::decition::Quintic_polynomial::calc_second_derivative(double t){
+    double xt = 2*this->a2 + 6*this->a3 * t + 12*this->a4 * t*t + 20*this->a5 * t*t*t;
+    return xt;
+}
+
+double iv::decition::Quintic_polynomial::calc_third_derivative(double t){
+    double xt = 6*this->a3 + 24*this->a4 * t + 60*this->a5 * t*t;
+    return xt;
+}
+
+
+
+iv::decition::Quartic_polynomial::Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T){
+    //计算四次多项式系数
+    this->xs = xs;
+    this->vxs = vxs;
+    this->axs = axs;
+    this->vxe = vxe;
+    this->axe = axe;
+
+    this->a0 = xs;
+    this->a1 = vxs;
+    this->a2 = axs / 2.0;
+
+    MatrixXd A(2, 2);
+    MatrixXd b(2, 1);
+    MatrixXd x(2, 1);
+
+    A << 3*pow(T, 2), 4*pow(T, 3),
+         6*T,         12*T*T ;
+    b << vxe - a1 - 2*a2*T,
+         axe - 2*a2;
+    x=A.colPivHouseholderQr().solve(b);
+
+    this->a3 = x(0,0);
+    this->a4 = x(1,0);
+}
+
+iv::decition::Quartic_polynomial::~Quartic_polynomial(){
+}
+
+double iv::decition::Quartic_polynomial::calc_point(double t){
+    double xt = this->a0 + this->a1*t + this->a2*t*t + this->a3*t*t*t + this->a4*t*t*t*t;
+    return xt;
+}
+
+double iv::decition::Quartic_polynomial::calc_first_derivative(double t){
+    double xt = this->a1 + 2*this->a2*t + 3*this->a3*t*t + 4*this->a4*t*t*t;
+    return xt;
+}
+
+double iv::decition::Quartic_polynomial::calc_second_derivative(double t){
+    double xt = 2*this->a2 + 6*this->a3*t + 12*this->a4*t*t;
+    return xt;
+}
+
+double iv::decition::Quartic_polynomial::calc_third_derivative(double t){
+    double xt = 6*this->a3 + 24*this->a4*t;
+    return xt;
+}
+
+bool iv::decition::FrenetPlanner::comp(const iv::decition::Frenet_path &a, const iv::decition::Frenet_path &b){
+    return a.cf < b.cf;
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/frenet_planner.h

@@ -0,0 +1,159 @@
+#ifndef FRENET_PLANNER_H
+#define FRENET_PLANNER_H
+
+#include "base_planner.h"
+
+namespace iv{
+namespace decition{
+
+    //x，y分别为frenet轨迹点相对于车辆的x，y轴坐标
+    //s，d分别为frenet轨迹点相对于frenet坐标系的s，d轴坐标值
+    //tangent_Ang为轨迹点在frenet坐标系的s轴的投影处的 切线 与车辆坐标系x轴之间的夹角。
+    //tangent_Ang用于分解计算车辆分别在s轴、d轴方向上的运动速度、加速度等。
+    struct FrenetPoint{
+        double x;
+        double y;
+        double s;
+        double d;
+        double tangent_Ang;
+    };
+
+
+
+    class Quintic_polynomial
+    {
+    public:
+        Quintic_polynomial(double xs, double vxs, double axs, double xe, double vxe, double axe, double T);
+        ~Quintic_polynomial();
+    private:
+        // 计算五次多项式系数
+        double xs;    //d0
+        double vxs;   //d.0
+        double axs;   //d..0
+        double xe;    //d1
+        double vxe;   //d.1
+        double axe;   //d..1
+
+        double a0;
+        double a1;
+        double a2;
+        double a3;
+        double a4;
+        double a5;
+
+    public:
+        double calc_point(double t);
+        double calc_first_derivative(double t);
+        double calc_second_derivative(double t);
+        double calc_third_derivative(double t);
+    };
+
+
+    class Quartic_polynomial
+    {
+    public:
+        Quartic_polynomial(double xs, double vxs, double axs, double vxe, double axe, double T);
+        ~Quartic_polynomial();
+
+    private:
+        //计算四次多项式系数
+        double xs;
+        double vxs;
+        double axs;
+        double vxe;
+        double axe;
+
+        double a0;
+        double a1;
+        double a2;
+        double a3;
+        double a4;
+
+    public:
+        double calc_point(double t);
+        double calc_first_derivative(double t);
+        double calc_second_derivative(double t);
+        double calc_third_derivative(double t);
+    };
+
+
+    class Frenet_path
+    {
+    public:
+        std::vector<double> t;
+        std::vector<double> d;
+        std::vector<double> d_d;
+        std::vector<double> d_dd;
+        std::vector<double> d_ddd;
+        std::vector<double> s;
+        std::vector<double> s_d;
+        std::vector<double> s_dd;
+        std::vector<double> s_ddd;
+        int roadflag = -1;  //标记每一条frenet路径，是以哪一条道路为目标道路的
+        double cd = 0.0;
+        double cv = 0.0;
+        double cf = 0.0;
+
+        std::vector<double> x;
+        std::vector<double> y;
+        std::vector<double> yaw;
+        std::vector<double> ds;
+        std::vector<double> c;
+    };
+
+    class FrenetPlanner : public BasePlanner{
+    public:
+        int aimRoadByFrenet = -1;          //记录由frenet规划选择出来的道路序号
+        int roadNow = -1;                  //记录避障时车辆当前道路序号,规划路径时，避开此条道路
+        double leftWidth = 0.0;            //中心线左边偏移距离，为负值
+        double rightWidth = 0.0;           //中心线右边偏移距离，为正值
+        GPS_INS now_gps_ins;               //实时gps信息
+        std::vector<GPSData> gpsMapLine;   //地图数据点
+        int PathPoint = 0;                 //地图路线中距离车辆位置最近的一个点的序号
+        iv::LidarGridPtr lidarGridPtr;     //激光雷达信息网格
+
+    public:
+        FrenetPlanner();
+        ~FrenetPlanner();
+
+        /**
+         * @brief iv::decition::FrenetPlanner::getPath
+         * 采用的基于frenet坐标系的最优局部路径生成方法，可实现从当前车道变换到另一车道之间的路径生成。还可实现车辆在路线旁起步的“有迹可循”。
+         * 计算的是，在车辆到达 原始地图路径偏移offset距离的车道 之前，frenet下规划的路径。
+         *
+         * @param now_gps_ins          实时gps信息
+         * @param gpsMapLine           地图数据点
+         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
+         * @param lidarGridPtr         激光雷达信息网格
+         * @return                     返回一条车辆坐标系下的路径
+         */
+        std::vector<iv::Point2D> getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
+        int chooseRoadByFrenet(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offsetL, double offsetR, iv::LidarGridPtr lidarGridPtr,int roadNow);
+        static iv::decition::FrenetPoint XY2Frenet(double x, double y, const std::vector<Point2D>& gpsTracet);
+        static void Frenet2XY(double *x, double *y, double s, double d, const std::vector<iv::decition::FrenetPoint>& frenetTrace);
+        static double distance(double x1, double y1, double x2, double y2);
+        static int ClosestWaypoint(double x, double y, const std::vector<Point2D>& gpsTrace);
+        static FrenetPoint getFrenetfromXY(double x, double y, const std::vector<iv::Point2D>& gpsTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+        static void getXYfromFrenet(double *res_x, double *res_y, double s, double d, const std::vector<iv::decition::FrenetPoint>& frenetTrace,const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+        static bool comp(const iv::decition::Frenet_path &a,const iv::decition::Frenet_path &b);
+
+    private:
+        std::vector<iv::Point2D> getGpsTrace_AfterCalcFrenetTrace(const std::vector<iv::Point2D>& gpsTrace,double realsecSpeed);
+        std::vector<iv::Point2D> frenet_optimal_planning(FrenetPoint car_now_frenet_point,
+                                                         const std::vector<FrenetPoint>& frenet_s,
+                                                         double c_speed, double c_d_d, double c_d_dd);
+        std::vector<Frenet_path> calc_frenet_paths(double c_speed, double c_d, double c_d_d, double c_d_dd, double s0);
+        void calc_global_paths(std::vector<Frenet_path>& fplist,const std::vector<FrenetPoint>& frenet_s);
+        void calc_global_single_path(Frenet_path& fp,const std::vector<FrenetPoint>& frenet_s);
+        std::vector<Frenet_path> check_paths(const std::vector<Frenet_path>& fplist);
+        bool check_single_path(const Frenet_path &fp);
+        bool check_collision(const Frenet_path &frenet_path);
+
+        std::vector<Point2D> frenet_path_to_gpsTrace(const Frenet_path& frenet_path);
+    };
+
+}
+}
+
+#endif // FRENET_PLANNER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/lane_change_planner.cpp

@@ -0,0 +1,197 @@
+#include <decition/adc_planner/lane_change_planner.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/adc_tools/parameter_status.h>
+#include <common/constants.h>
+#include "common/tracepointstation.h"
+
+#include <math.h>
+
+iv::decition::LaneChangePlanner::LaneChangePlanner(){
+    this->planner_id = 0;
+    this->planner_name = "LaneChange";
+}
+
+iv::decition::LaneChangePlanner::~LaneChangePlanner(){
+}
+
+/**
+ * @brief iv::decition::LaneChangePlanner::getPath
+ * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
+ *
+ * @param now_gps_ins          实时gps信息
+ * @param gpsMapLine           地图数据点
+ * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+ * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
+ * @param lidarGridPtr         激光雷达信息网格
+ * @return                     返回一条车辆坐标系下的路径
+ */
+std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
+    std::vector<iv::Point2D> trace;
+    trace = this->getGpsTrace(now_gps_ins,gpsMapLine,PathPoint);
+    if(offset!=0){
+        trace = this->getGpsTraceOffset(trace,offset);
+    }
+    return trace;
+}
+
+bool iv::decition::LaneChangePlanner::checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim){
+    int roadNow = ServiceParameterStatus.now_road_num;
+    if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
+            (obsDisVector[roadAim]>0 && obsDisVector[roadAim]<15))
+    {
+        return false;
+    }
+
+    if (roadAim-roadNow>1)
+    {
+        for (int i = roadNow+1; i < roadAim; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow-roadAim>1)
+    {
+        for (int i = roadNow-1; i >roadAim; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint){
+    std::vector<iv::Point2D> trace;
+
+    if (gpsMapLine.size() > 400 && PathPoint >= 0) {
+        for (int i = PathPoint; i < PathPoint + 600; i++)
+        {
+            int index = i % gpsMapLine.size();
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
+//            pt.x += offset_real * 0.032;
+            pt.v1 = (*gpsMapLine[index]).speed_mode;
+            pt.v2 = (*gpsMapLine[index]).mode2;
+            pt.roadMode=(*gpsMapLine[index]).roadMode;
+
+//            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+//            {
+//                readyZhucheMode = true;
+//                DecideGps00::zhuchePointNum = index;
+//            }
+
+
+
+//            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+//            {
+//                readyZhucheMode = true;
+//                DecideGps00::zhuchePointNum = index;
+//            }
+
+//            //csvv7
+//            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
+//            {
+//                readyParkMode = true;
+//                DecideGps00::finishPointNum = index;
+//            }
+
+
+            switch (pt.v1)
+            {
+            case 0:
+                pt.speed = 36;
+                break;
+            case 1:
+                pt.speed = 25;
+                break;
+            case 2:
+                pt.speed =25;
+                break;
+            case 3:
+                pt.speed = 20;
+                break;
+            case 4:
+                pt.speed =18;
+                break;
+            case 5:
+                pt.speed = 18;
+                break;
+            case 7:
+                pt.speed = 10;
+                break;
+            case 22:
+                pt.speed = 5;
+                break;
+            case 23:
+                pt.speed = 5;
+                break;
+            default:
+                break;
+            }
+            trace.push_back(pt);
+        }
+    }
+    return trace;
+}
+
+std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTraceOffset(const std::vector<Point2D>& gpsTrace, double offset){
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = std::max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= std::min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + M_PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + M_PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            ptLeft.roadMode = gpsTrace[j].roadMode;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - M_PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - M_PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+            ptRight.roadMode = gpsTrace[j].roadMode;
+            trace.push_back(ptRight);
+        }
+    }
+    return trace;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/lane_change_planner.h

@@ -0,0 +1,38 @@
+#ifndef LANE_CHANGE_PLANNER_H
+#define LANE_CHANGE_PLANNER_H
+
+#include "base_planner.h"
+
+namespace iv{
+namespace decition{
+    class LaneChangePlanner : public BasePlanner{
+    public:
+        LaneChangePlanner();
+        ~LaneChangePlanner();
+
+
+        /**
+         * @brief iv::decition::LaneChangePlanner::getPath
+         * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
+         *
+         * @param now_gps_ins          实时gps信息
+         * @param gpsMapLine           地图数据点
+         * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
+         * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
+         * @param lidarGridPtr         激光雷达信息网格
+         * @return                     返回一条车辆坐标系下的路径
+         */
+        std::vector<iv::Point2D> getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr);
+
+        bool checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim);
+
+    private:
+        std::vector<iv::Point2D> getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint);
+        std::vector<iv::Point2D> getGpsTraceOffset(const std::vector<Point2D>& gpsTrace, double offset);
+
+    };
+}
+}
+
+
+#endif // LANE_CHANGE_PLANNER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/spline_planner.cpp

@@ -0,0 +1,237 @@
+#include "spline_planner.h"
+
+#include <decition/adc_tools/transfer.h>
+#include <common/car_status.h>
+#include <decition/adc_tools/parameter_status.h>
+#include <decition/decide_gps_00.h>
+#include <decition/adc_planner/lane_change_planner.h>
+
+#include<Eigen/Dense>
+
+using namespace std;
+using namespace Eigen;
+QVector<QPointF> ctrlPoints;  // 控制点
+QVector<QPointF> curvePoints; // 曲线上的点
+
+iv::decition::SplinePlanner::SplinePlanner(){
+    this->planner_id = 5;
+    this->planner_name = "Spline";
+    this->lidarGridPtr = NULL;
+}
+
+iv::decition::SplinePlanner::~SplinePlanner(){
+    free(this->lidarGridPtr);
+}
+std::vector<iv::Point2D> iv::decition::SplinePlanner::chooseRoadBySpline(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint,std::vector<iv::Point2D> obsSpline,int avoidX,int nowX){
+
+    double s_obs_current,d_obs_current,s_obs_current_relative;
+    double s_obs_avoid_relative,d_obs_avoid,s_obs_avoid;
+    double s_offset,d_offset;
+    std::vector<iv::Point2D> gpsTrace;
+    for(unsigned int j=0;j<obsSpline.size();j++){
+        if(fabs(obsSpline[j].d - avoidX) <= 1.0e-6){
+                    s_obs_avoid_relative=obsSpline[j].s;
+                    d_obs_avoid=-obsSpline[j].d;
+        }
+        if(fabs(obsSpline[j].d - nowX)<= 1.0e-6){
+                    s_obs_current_relative=obsSpline[j].s;
+                    if(s_obs_current_relative>40)
+                        s_obs_current_relative=40;
+        }
+    }
+    iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+//            if(s_obs_avoid_relative>=s_obs_current_relative*2){
+//                            s_obs_avoid=now_s_d.s+s_obs_current_relative*2;
+//            }else{
+//                            s_obs_avoid=now_s_d.s+s_obs_avoid_relative;
+//            }
+//    s_obs_avoid=now_s_d.s+s_obs_current_relative;//避障路线终点s值
+//    s_obs_current=now_s_d.s;
+    s_obs_avoid=s_obs_current_relative;//避障路线终点s值
+    s_obs_current=0;
+    d_obs_current=now_s_d.d;
+    s_offset=s_obs_avoid-s_obs_current;
+    d_offset=d_obs_avoid-d_obs_current;
+    ctrlPoints={QPointF(s_obs_current,d_obs_current),QPointF(s_obs_current+s_offset/4,d_obs_current),QPointF(s_obs_current+s_offset/2,(d_obs_current+(d_offset)/4)),
+                QPointF(s_obs_current+s_offset/2,(d_obs_avoid-(d_offset)/4)),QPointF(s_obs_avoid-s_offset/4,d_obs_avoid),QPointF(s_obs_avoid,d_obs_avoid)};
+
+
+//            now_s_d.s=0;
+//            ctrlPoints.clear();
+//            QPointF point=QPointF(0,0);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(10,0);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(25,-0.5);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(25,-1.5);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(40,-2);
+//            ctrlPoints.push_back(point);
+//            point=QPointF(50,-2);
+//            ctrlPoints.push_back(point);
+//            ctrlPoints={QPointF(now_s_d.s+0,0),QPointF(now_s_d.s+10,0),QPointF(now_s_d.s+25,-0.5),
+//                        QPointF(now_s_d.s+25,-1.5),QPointF(now_s_d.s+40,-2),QPointF(now_s_d.s+50,-2)};
+
+
+            generateCurve();
+
+double start=GetTickCount();
+            gpsTrace.clear();
+            for(int i=0;i<curvePoints.size();i++){
+                double s_transfer=curvePoints[i].x()+now_s_d.s;
+
+                iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s_transfer, curvePoints[i].y(),PathPoint);
+
+                gpsTrace.push_back(gpsTracePoint);
+
+            }
+double end=GetTickCount();
+
+            double period=end-start;
+            std::cout<<"===================period========================"<<period<<       std::endl;
+            //std::cout<<"===================curvesize========================"<<curvePoints.size()<<       std::endl;
+            /*for(double s=s_obs_avoid;s<(*gpsMapLine[gpsMapLine.size()-1]).frenet_s;s+=0.1){
+                iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s, d_obs_avoid);
+                gpsTrace.push_back(gpsTracePoint);
+            }*/
+            return gpsTrace;
+
+
+}
+void iv::decition::SplinePlanner::generateCurve()
+{
+    int n=5;//控制点个数，从0开始计数，5代表有6个控制点
+    int k=4;
+    float d=100;//3.5
+
+    n=ctrlPoints.size()-1;
+
+//    QVector<QPointF> Points={QPointF(0,d/2),QPointF(40,d/2),QPointF(60,d/2+0.5),
+//                             QPointF(100,d-0.5),QPointF(160,d),QPointF(200,d)};
+
+    vector<float> NodeVector;
+    vector<float> Bik(n+1);
+    if(n>=3){
+        NodeVector=U_quasi_uniform(n, k-1);
+        curvePoints.clear();
+        for(float u=0;u<=1-0.005;u+=0.005){
+            for(int i=0;i<=n;i++){
+                Bik[i] = BaseFunction(i, k-1 , u, NodeVector);
+                //cout<<Bik[i]<<','<<Bik[i]<<endl;
+            }
+            QPointF tmp(0,0);
+            for(int i=0;i<=n;i++){
+                QPointF t = ctrlPoints[i];
+                t*=Bik[i];
+                tmp+=t;
+            }
+            //cout<<tmp.x()<<','<<tmp.y()<<endl;
+            curvePoints.push_back(tmp);
+        }
+    }
+}
+
+vector<float> iv::decition::SplinePlanner::U_quasi_uniform(int n, int k){
+    vector<float>  NodeVector(n+k+2);
+    int piecewise = n - k + 1;
+
+    if(piecewise==1){
+        for (int i = k+1;i<= n+k+1;i++){
+            NodeVector[i]= 1;
+        }
+    }else{
+        int flag=1;
+        while(flag != piecewise){
+            NodeVector[k+flag]= NodeVector[k + flag-1] + 1/(float)piecewise;
+            flag = flag + 1;
+        }
+        for (int i = n+1;i<= n+k+1;i++){
+            NodeVector[i]= 1;
+        }
+    }
+
+    return NodeVector;
+}
+
+float iv::decition::SplinePlanner::BaseFunction(int i, int k , float u, vector<float> NodeVector ){
+        float Bik_u=0;
+        float Length1=0,Length2=0;
+        if(k==0){
+            if ((u >= NodeVector[i]) && (u < NodeVector[i+1]))
+                Bik_u = 1;
+            else
+                Bik_u = 0;
+        }else{
+            Length1 = NodeVector[i+k] - NodeVector[i];
+            Length2 = NodeVector[i+k+1] - NodeVector[i+1];
+            if(Length1==0)
+                Length1=1;
+            if(Length2==0)
+                Length2=1;
+            Bik_u = (u - NodeVector[i]) / Length1 * BaseFunction(i, k-1, u, NodeVector)
+                    + (NodeVector[i+k+1] - u) / Length2 * BaseFunction(i+1, k-1, u, NodeVector);
+        }
+        return Bik_u;
+}
+
+
+
+
+
+double iv::decition::SplinePlanner::N(int k, int i, double u)
+{
+    switch(k)
+    {
+    case 1:
+        return N1(i,u);
+    case 2:
+        return N2(i,u);
+    case 3:
+        return N3(i,u);
+    }
+}
+
+double iv::decition::SplinePlanner::N1(int i, double u)
+{
+    double t = u-i;
+
+    if(0<=t && t<1)
+        return t;
+    if(1<=t && t<2)
+        return 2-t;
+    return 0;
+}
+
+double iv::decition::SplinePlanner::N2(int i, double u)
+{
+    double t = u-i;
+
+    if(0<=t && t<1)
+        return 0.5*t*t;
+    if(1<=t && t<2)
+        return 3*t-t*t-1.5;
+    if(2<=t && t<3)
+        return 0.5*pow(3-t,2);
+    return 0;
+}
+
+double iv::decition::SplinePlanner::N3(int i, double u)
+{
+    double t = u-i;
+    double a = 1.0/6.0;
+
+    if(0<=t && t<1)
+        return a*t*t*t;
+    if(1<=t && t<2)
+        return a*(-3*pow(t-1,3) + 3*pow(t-1,2) + 3*(t-1) +1);
+    if(2<=t && t<3)
+        return a*(3*pow(t-2,3) - 6*pow(t-2,2) +4);
+    if(3<=t && t<4)
+        return a*pow(4-t,3);
+    return 0;
+}
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_planner/spline_planner.h

@@ -0,0 +1,54 @@
+#ifndef SPLINE_PLANNER_H
+#define SPLINE_PLANNER_H
+
+#include "base_planner.h"
+#include <QWidget>
+#include <vector>
+#include <QVector>
+#include <QCursor>
+#include <QPainter>
+#include <QPaintEvent>
+#include <cmath>
+
+using namespace std;
+
+namespace iv{
+namespace decition{
+
+class SplinePlanner : public BasePlanner{
+public:
+
+    iv::LidarGridPtr lidarGridPtr;     //激光雷达信息网格
+
+public:
+    SplinePlanner();
+    ~SplinePlanner();
+
+    void generateCurve();
+    std::vector<iv::Point2D> chooseRoadBySpline(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint,std::vector<iv::Point2D> obsSpline,int avoidX,int nowX);
+
+
+
+private:
+    vector<float> U_quasi_uniform(int n, int k);
+    float BaseFunction(int i, int k , float u, vector<float> NodeVector );
+    double N(int k, int i, double u);
+    double N1(int i, double u);
+    double N2(int i, double u);
+    double N3(int i, double u);
+
+};
+
+
+
+
+
+}
+}
+
+
+
+
+
+
+#endif // SPLINE_PLANNER_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/PolynomialRegression.h

@@ -0,0 +1,214 @@
+// Polynomial Regression (Quadratic Fit) in C++
+#ifndef POLYNOMIALREGRESSION_H
+#define POLYNOMIALREGRESSION_H
+
+/**
+ * PURPOSE:
+ *
+ *  Polynomial Regression aims to fit a non-linear relationship to a set of
+ *  points. It approximates this by solving a series of linear equations using
+ *  a least-squares approach.
+ *
+ *  We can model the expected value y as an nth degree polynomial, yielding
+ *  the general polynomial regression model:
+ *
+ *  y = a0 + a1 * x + a2 * x^2 + ... + an * x^n
+ *
+ * LICENSE:
+ *
+ * MIT License
+ *
+ * Copyright (c) 2020 Chris Engelsma
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * @authors Chris Engelsma, Mohammad Raziei
+ */
+#include <vector>
+#include <stdlib.h>
+#include <cmath>
+#include <stdexcept>
+
+class PolynomialRegression
+{
+public:
+    PolynomialRegression();
+    virtual ~PolynomialRegression(){};
+
+    template <class TYPE>
+    static std::vector<TYPE> fit(
+         const std::vector<TYPE>& x,
+         const std::vector<TYPE>& y,
+         const int& order);
+
+    template <class TYPE>
+    static std::vector<TYPE> eval(
+         const std::vector<TYPE>& coeffs,
+         const std::vector<TYPE>& x);
+    template <class TYPE>
+    static TYPE eval(
+         const std::vector<TYPE>& coeffs,
+         const TYPE& x);
+
+    template <class TYPE, class T>
+    static T polyFit(
+         const std::vector<TYPE>& x,
+         const std::vector<TYPE>& y,
+         const T& x2Compute,
+         const int& order);
+};
+
+template <class TYPE>
+std::vector<TYPE> PolynomialRegression::fit(
+     const std::vector<TYPE>& x,
+     const std::vector<TYPE>& y,
+     const int& order)
+{
+    /// The size of xValues and yValues should be same
+    if (x.size() != y.size())
+        throw std::runtime_error("The size of x & y arrays are different");
+
+    /// The size of xValues and yValues cannot be 0, should not happen
+    if (x.size() == 0 || y.size() == 0)
+        throw std::runtime_error("The size of x or y arrays is 0");
+
+    size_t N = x.size();
+    int n = order;
+    int np1 = n + 1;
+    int np2 = n + 2;
+    int tnp1 = 2 * n + 1;
+    TYPE tmp;
+
+    /// X = vector that stores values of sigma(xi^2n)
+    std::vector<TYPE> X(tnp1);
+    for (int i = 0; i < tnp1; ++i)
+    {
+        X[i] = 0;
+        for (size_t j = 0; j < N; ++j)
+            X[i] += (TYPE)pow(x[j], i);
+    }
+
+    /// a = vector to store final coefficients.
+    std::vector<TYPE> a(np1);
+
+    /// B = normal augmented matrix that stores the equations.
+    std::vector<std::vector<TYPE>> B(np1, std::vector<TYPE>(np2, 0));
+
+    for (int i = 0; i <= n; ++i)
+        for (int j = 0; j <= n; ++j)
+            B[i][j] = X[i + j];
+
+    /// Y = vector to store values of sigma(xi^n * yi)
+    std::vector<TYPE> Y(np1);
+    for (int i = 0; i < np1; ++i)
+    {
+        Y[i] = (TYPE)0;
+        for (size_t j = 0; j < N; ++j)
+        {
+            Y[i] += (TYPE)pow(x[j], i) * y[j];
+        }
+    }
+
+    /// Load values of Y as last column of B
+    for (int i = 0; i <= n; ++i)
+        B[i][np1] = Y[i];
+
+    n += 1;
+    int nm1 = n - 1;
+
+    /// Pivotisation of the B matrix.
+    for (int i = 0; i < n; ++i)
+        for (int k = i + 1; k < n; ++k)
+            if (B[i][i] < B[k][i])
+                for (int j = 0; j <= n; ++j)
+                {
+                    tmp = B[i][j];
+                    B[i][j] = B[k][j];
+                    B[k][j] = tmp;
+                }
+
+    /// Performs the Gaussian elimination.
+    /// (1) Make all elements below the pivot equals to zero
+    ///     or eliminate the variable.
+    for (int i = 0; i < nm1; ++i)
+        for (int k = i + 1; k < n; ++k)
+        {
+            TYPE t = B[k][i] / B[i][i];
+            for (int j = 0; j <= n; ++j)
+                B[k][j] -= t * B[i][j]; // (1)
+        }
+
+    /// Back substitution.
+    /// (1) Set the variable as the rhs of last equation
+    /// (2) Subtract all lhs values except the target coefficient.
+    /// (3) Divide rhs by coefficient of variable being calculated.
+    for (int i = nm1; i >= 0; --i)
+    {
+        a[i] = B[i][n]; // (1)
+        for (int j = 0; j < n; ++j)
+            if (j != i)
+                a[i] -= B[i][j] * a[j]; // (2)
+        a[i] /= B[i][i]; // (3)
+    }
+
+    //    std::vector<TYPE> coeffs(a.size());
+    //    for (size_t i = 0; i < a.size(); ++i)
+    //        coeffs[i] = a[i];
+
+    return a; /// return coefficients
+}
+
+template <class TYPE>
+TYPE PolynomialRegression::eval(
+     const std::vector<TYPE>& coeffs,
+     const TYPE& x)
+{
+    TYPE sum = 0;
+    TYPE val = 1;
+    size_t coeffSize = coeffs.size();
+    for (size_t i = 0; i < coeffSize; ++i)
+    {
+        sum += coeffs[i] * val;
+        val *= x;
+    }
+    return sum;
+}
+
+template <class TYPE>
+std::vector<TYPE> PolynomialRegression::eval(
+     const std::vector<TYPE>& coeffs,
+     const std::vector<TYPE>& x)
+{
+    size_t xSize = x.size();
+    std::vector<TYPE> ret(x.size());
+    for (size_t i = 0; i < xSize; ++i)
+        ret[i] = eval(coeffs, x[i]);
+    return ret;
+}
+
+template <class TYPE, class T>
+T PolynomialRegression::polyFit(
+     const std::vector<TYPE>& x,
+     const std::vector<TYPE>& y,
+     const T& x2Compute,
+     const int& order)
+{
+    return eval(fit(x, y, order), x2Compute);
+}
+#endif /// POLYNOMIALREGRESSION_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/compute_00.cpp

@@ -0,0 +1,2630 @@
+#include <decition/adc_tools/compute_00.h>
+#include <decition/decide_gps_00.h>
+#include <decition/adc_tools/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/adc_tools/transfer.h>
+#include <common/constants.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+
+#include "common/tracepointstation.h"
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+using namespace std;
+
+#define PI (3.1415926535897932384626433832795)
+
+iv::decition::Compute00::Compute00() {
+
+}
+iv::decition::Compute00::~Compute00() {
+
+}
+
+
+double iv::decition::Compute00::angleLimit = 700;
+double iv::decition::Compute00::lastEA = 0;
+double iv::decition::Compute00::lastEP = 0;
+double iv::decition::Compute00::decision_Angle = 0;
+double iv::decition::Compute00::lastAng = 0;
+int iv::decition::Compute00::lastEsrID = -1;
+int  iv::decition::Compute00::lastEsrCount = 0;
+int iv::decition::Compute00::lastEsrIDAvoid = -1;
+int  iv::decition::Compute00::lastEsrCountAvoid = 0;
+
+iv::GPS_INS  iv::decition::Compute00::nearTpoint;
+iv::GPS_INS  iv::decition::Compute00::farTpoint;
+double  iv::decition::Compute00::bocheAngle;
+
+
+
+iv::GPS_INS  iv::decition::Compute00::dTpoint0;
+iv::GPS_INS  iv::decition::Compute00::dTpoint1;
+iv::GPS_INS  iv::decition::Compute00::dTpoint2;
+iv::GPS_INS  iv::decition::Compute00::dTpoint3;
+double  iv::decition::Compute00::dBocheAngle;
+
+
+std::vector<int> lastEsrIdVector;
+std::vector<int> lastEsrCountVector;
+
+
+
+int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
+{
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+    static double FrontTotalFive=0,FrontAveFive=0,BackAveFive=0,BackTotalFive=0;
+    static int FrontCount=0,BackCount=0;
+    static int CurrentIndex=0,MarkingIndex=0,MarkingCount=0;
+    int MarkedIndex=0,CurveContinue=0;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        if((*gpsMap[i]).roadMode!=6)
+                (*gpsMap[i]).roadMode=5;
+    }
+    for (int j = startIndex; j < (endIndex-40); j+=40)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        for(int front_k=i;front_k<i+20;front_k++)
+        {
+            if(fabs(((*gpsMap[front_k]).ins_heading_angle)-((*gpsMap[i]).ins_heading_angle))<10)
+            {
+                   FrontTotalFive+=(*gpsMap[front_k]).ins_heading_angle;
+                   FrontCount++;
+            }
+
+        }
+        i+=20;
+        FrontAveFive=FrontTotalFive/FrontCount;
+        FrontTotalFive=0;
+        FrontCount=0;
+        for(int back_k=i;back_k<i+20;back_k++)
+        {
+            if(fabs((*gpsMap[back_k]).ins_heading_angle-(*gpsMap[i]).ins_heading_angle)<10)
+            {
+                   BackTotalFive+=(*gpsMap[back_k]).ins_heading_angle;
+                   BackCount++;
+            }
+
+        }
+        i+=20;
+        CurrentIndex=i-1;
+        BackAveFive=BackTotalFive/BackCount;
+        BackTotalFive=0;
+        BackCount=0;
+        if(fabs(FrontAveFive-BackAveFive)<50)
+        {
+                   if(fabs(FrontAveFive-BackAveFive)>4)
+                   {
+                        CurveContinue+=1;
+                        if(CurveContinue>=1)
+                        {
+                            MarkingCount=0;
+                            for(MarkingIndex=CurrentIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
+                            {
+                                if((*gpsMap[MarkingIndex]).roadMode!=6)
+                                {
+                                if(MarkingCount<150)
+                                {
+                                     if((BackAveFive-FrontAveFive)<=4.0)
+                                     {
+                                           (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
+                                     }
+                                     else if((BackAveFive-FrontAveFive)>4.0)
+                                     {
+                                           (*gpsMap[MarkingIndex]).roadMode=15;
+                                     }
+
+                                } //else if((MarkingCount>=100)&&(MarkingCount<150)){(*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米,1}
+                                else if((MarkingCount>=150)&&(MarkingCount<320))
+                                {
+                                     (*gpsMap[MarkingIndex]).roadMode=5;   //低速，20米,5
+                                }
+                                else if((MarkingCount>=320)&&(MarkingCount<620))
+                                {
+                                     (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
+                                }
+                                else if(MarkingCount>=620)
+                                {
+                                     (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
+                                }
+                                }
+
+                                MarkingCount++;
+                            }
+                            MarkedIndex=CurrentIndex;
+                        }
+                   }
+                   else if(fabs(FrontAveFive-BackAveFive)<=4)
+                   {
+                        CurveContinue=0;
+                   }
+        }
+        FrontAveFive=0;
+        BackAveFive=0;
+    }
+
+    if(MarkedIndex<endIndex)
+    {
+        MarkingCount=0;
+        for(MarkingIndex=endIndex;MarkingIndex>=MarkedIndex;MarkingIndex--)
+        {
+            if((*gpsMap[MarkingIndex]).roadMode!=6)
+            {
+            if(MarkingCount<100)
+            {
+                if((BackAveFive-FrontAveFive)<3)
+                {
+                      (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
+                }
+                else if((BackAveFive-FrontAveFive)>3)
+                {
+                      (*gpsMap[MarkingIndex]).roadMode=15;
+                }
+            }
+            else if((MarkingCount>=100)&&(MarkingCount<150))
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=18;   //超低速，10米
+            }
+            else if((MarkingCount>=150)&&(MarkingCount<320))
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=5;   //低速，30米
+            }
+            else if((MarkingCount>=320)&&(MarkingCount<620))
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=0;   //常速，60米
+            }
+            else if(MarkingCount>=620)
+            {
+                 (*gpsMap[MarkingIndex]).roadMode=11;   //高速/疯狂加速，大于60米
+            }
+             }
+            MarkingCount++;
+        }
+    }
+
+
+    return 1;
+}
+
+
+int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
+    double distance,deltaphi;
+    //x,y,phi in rad
+    doubledata.clear();
+    for (int i = 0; i < MapTrace.size(); i++) {   //                                       1225/14:25
+                 std::vector<double> temp;
+                 temp.clear();
+                 temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push四个空量，然后就可以给量赋值了
+                 doubledata.push_back(temp);
+                 doubledata[i][0] = MapTrace.at(i)->gps_x;
+                 doubledata[i][1] = MapTrace.at(i)->gps_y;
+                 doubledata[i][2] = (MapTrace.at(i)->ins_heading_angle) / 180 * PI;
+                 doubledata[i][3]=0;
+                 doubledata[i][4]=0;
+    }
+    // compute delta///////////////////////////////////////////////////////////////
+    for (int i = 0; i < doubledata.size()-10; i++)
+    {
+                deltaphi=doubledata[i+10][2]-doubledata[i][2];
+                if (deltaphi>PI)
+                        {deltaphi=deltaphi-2*PI; }
+                if (deltaphi<-PI)
+                        {deltaphi=deltaphi+2*PI;}
+                distance=sqrt( pow((doubledata[i+10][0]-doubledata[i][0]),2)+pow((doubledata[i+10][1]-doubledata[i][1]),2));
+                doubledata[i][3]=-atan( 4.0* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
+    }
+                doubledata[doubledata.size()-1][3] =  doubledata[doubledata.size()-2][3];
+
+    //delta filter
+    for(int j=10;j<doubledata.size()-10;j++)
+    {
+        double delta_sum=0;
+        for(int k=j-10;k<j+10;k++)
+        {
+            delta_sum+= doubledata[k][3];
+        }
+        doubledata[j][3]=delta_sum/20;
+    }
+}
+
+
+/*int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
+{
+                int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
+                double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+                double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+                double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
+                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
+                getMapDelta(MapTrace);
+//                for(int i=0;i<doubledata.size();i++)
+//                {
+//                    if((doubledata[i][3]>-0.4)&&(doubledata[i][3]<0.4)){
+//                        MapTrace[i]->roadMode=0;
+//                    }else if(((doubledata[i][3]>0.4)&&(doubledata[i][3]<4))||((doubledata[i][3]>-4)&&(doubledata[i][3]<-0.4))){
+//                        MapTrace[i]->roadMode=5;
+//                    }else if(((doubledata[i][3]>4)&&(doubledata[i][3]<10))||((doubledata[i][3]>-10)&&(doubledata[i][3]<-4))){
+//                        MapTrace[i]->roadMode=18;
+//                    }else if(((doubledata[i][3]>10))||((doubledata[i][3]<-10))){
+//                        MapTrace[i]->roadMode=14;
+//                    }
+//                }
+                for(int i=0;i<doubledata.size();i++)
+                {
+                    if((doubledata[i][3]>-0.26)&&(doubledata[i][3]<0.26)){
+                        MapTrace[i]->roadMode=5;
+                    }else if(((doubledata[i][3]>=0.26)&&(doubledata[i][3]<=1.0))||((doubledata[i][3]>=-1.0)&&(doubledata[i][3]<=-0.26))){
+                        MapTrace[i]->roadMode=18;
+                    }else if(((doubledata[i][3]>1.0))||((doubledata[i][3]<-1.0))){
+                        MapTrace[i]->roadMode=14;
+                    }
+                }
+                for(int i=0;i<MapTrace.size();i++)
+                {
+                    if(MapTrace[i]->roadMode==0){
+                        doubledata[i][4]=straightSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==5){
+                        doubledata[i][4]=straightCurveSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==18){
+                        mode0to5countSum=mode0to5count;
+                        doubledata[i][4]=Curve_SmallSpeed;
+
+                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
+                        int brake_distance=(int)brake_distance_double;
+                        int step_count=0;
+                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                        {
+                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                            for(int j=i;j>i-brake_distance;j--){
+                                doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+                                step_count++;
+                            }
+                        }else if(mode0to5countSum>0){
+                            for(int j=i;j>=i-mode0to5countSum;j--){
+                                doubledata[j][4]=Curve_SmallSpeed;
+                                step_count++;
+                            }
+                        }else{
+                            doubledata[i][4]=Curve_SmallSpeed;
+                        }
+                        mode0to5count=0;
+                        //mode0to18count++;
+                        mode18count++;
+                        mode18flash=mode18count;
+                    }else if(MapTrace[i]->roadMode==14){
+                        mode0to18countSum=mode0to5flash+mode18flash;
+                        mode18countSum=mode18count;
+                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        int brake_distanceLarge=(int)brake_distanceLarge_double;
+                        int brake_distance0to18=(int)brake_distance0to18_double;
+                        int step_count=0;
+                        doubledata[i][4]=Curve_LargeSpeed;
+
+                        if(mode18countSum>brake_distanceLarge)
+                        {
+                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                for(int j=i;j>i-brake_distanceLarge;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                    step_count++;
+                                }
+
+                        }else if(mode0to18countSum>brake_distance0to18){
+
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+                                for(int j=i;j>i-brake_distance0to18;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                    step_count++;
+                                }
+                        }else{
+                                doubledata[i][4]=Curve_LargeSpeed;
+                        }
+
+                        mode0to5count=0;
+                        mode18count=0;
+                        mode0to5flash=0;
+                        mode18flash=0;
+                    }
+                }
+
+                for(int i=0;i<MapTrace.size();i++){
+                    //将数据写入文件开始
+                    ofstream outfile;
+                    outfile.open("ctr0108003.txt", ostream::app);
+                    outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+                           <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<"num,curve"<< ","  <<i<< ","
+                           <<MapTrace[i]->mfCurvature<<endl;
+                    outfile.close();
+                    //将数据写入文件结束
+                }
+
+}*/
+
+int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
+{
+                int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
+                double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+                double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+                double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
+                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
+                getMapDelta(MapTrace);
+                for(int i=0;i<doubledata.size();i++)
+                {
+                    if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
+                        MapTrace[i]->roadMode=5;
+                    }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
+                        MapTrace[i]->roadMode=18;
+                    }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
+                        MapTrace[i]->roadMode=14;
+                    }
+                }
+                for(int i=0;i<MapTrace.size();i++)
+                {
+                    if(MapTrace[i]->roadMode==0){
+                        doubledata[i][4]=straightSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==5){
+                        doubledata[i][4]=straightCurveSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==18){
+                        mode0to5countSum=mode0to5count;
+                        doubledata[i][4]=Curve_SmallSpeed;
+
+                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
+                        int brake_distance=(int)brake_distance_double;
+                        int step_count=0;
+                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                        {
+                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                            for(int j=i;j>i-brake_distance;j--){
+                                doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+                                step_count++;
+                            }
+                        }else if(mode0to5countSum>0){
+                            for(int j=i;j>=i-mode0to5countSum;j--){
+                                doubledata[j][4]=Curve_SmallSpeed;
+                                step_count++;
+                            }
+                        }else{
+                            doubledata[i][4]=Curve_SmallSpeed;
+                        }
+                        mode0to5count=0;
+                        //mode0to18count++;
+                        mode18count++;
+                        mode18flash=mode18count;
+                    }else if(MapTrace[i]->roadMode==14){
+                        mode0to18countSum=mode0to5flash+mode18flash;
+                        mode18countSum=mode18count;
+                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        int brake_distanceLarge=(int)brake_distanceLarge_double;
+                        int brake_distance0to18=(int)brake_distance0to18_double;
+                        int step_count=0;
+                        doubledata[i][4]=Curve_LargeSpeed;
+
+                        if(mode18countSum>brake_distanceLarge)
+                        {
+                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                for(int j=i;j>i-brake_distanceLarge;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                    step_count++;
+                                }
+
+                        }else if(mode0to18countSum>brake_distance0to18){
+
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+                                for(int j=i;j>i-brake_distance0to18;j--){
+                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                    step_count++;
+                                }
+                        }else if(mode0to18countSum>0){
+                            for(int j=i;j>=i-mode0to18countSum;j--){
+                                doubledata[j][4]=Curve_LargeSpeed;
+                                step_count++;
+                            }
+                        }else{
+                                doubledata[i][4]=Curve_LargeSpeed;
+                        }
+
+                        mode0to5count=0;
+                        mode18count=0;
+                        mode0to5flash=0;
+                        mode18flash=0;
+                    }
+                }
+
+                /*for(int i=0;i<MapTrace.size();i++){
+                    //将数据写入文件开始
+                    ofstream outfile;
+                    outfile.open("ctr0108003.txt", ostream::app);
+                    outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+                           <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<"num,curve"<< ","  <<i<< ","
+                           <<MapTrace[i]->mfCurvature<<endl;
+                    outfile.close();
+                    //将数据写入文件结束
+                }*/
+
+}
+
+
+//首次找点
+
+int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    //	DecideGps00().minDis = iv::MaxValue;
+    float minDis = 10;
+    double maxAng = iv::MaxValue;
+
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+                )
+        {
+            index = i;
+            minDis = tmpdis;
+            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+
+    DecideGps00().maxAngle=maxAng;
+    DecideGps00().minDis=minDis;
+    return index;
+}
+
+//search pathpoint
+int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    float minDis = 10;
+    double maxAng = iv::MaxValue;
+    int map_size=gpsMap.size();
+    int preDistance=max(100,(int)(rp.speed*10));
+        preDistance=min(500,preDistance);
+
+    //int startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size));     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = min((int)(lastIndex + preDistance ),(int)(map_size-1));
+    int startIndex=0;
+    if(lastIndex>100){
+        startIndex = max((int)(lastIndex - 100),(int)(lastIndex-map_size));
+    }else{
+        startIndex=0;
+    }
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + map_size) % map_size;
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+            || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+            )
+        {
+            index = i;
+            minDis = tmpdis;
+            maxAng = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            maxAng = min(maxAng, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+
+
+    DecideGps00().maxAngle=maxAng;
+    DecideGps00().minDis=minDis;
+    return index;
+}
+
+
+double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x / average_y) / PI * 180;
+}
+
+
+
+double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x + avoidX / average_y) / PI * 180;
+}
+
+
+
+
+
+
+double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    if(transferPieriod&& !transferPieriod2){
+        DEang = 200;
+        DEPos = 150;
+    }
+
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+    if(changeRoad ||transferPieriod){
+        PreviewDistance=PreviewDistance+avoidX;
+    }
+    if(realSpeed <15){
+        PreviewDistance = max(4.0, realSpeed *0.4) ;
+    }
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+
+
+
+    EPos = gpsTrace[gpsIndex].x;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAveDef(farTrace);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+
+int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
+{
+    int index = 1;
+    double sumdis = 0;
+    while (index < gpsTrace.size() && sumdis < realSpeed)
+        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
+
+    if (index == gpsTrace.size())
+        return index - 1;
+
+    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
+        index--;
+    return index;
+}
+
+/*iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
+    }
+
+    ServiceCarStatus.obsTraceLeft.clear();
+    ServiceCarStatus.obsTraceRight.clear();
+
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+
+        TracePoint obsptleft(ptLeft.x,ptLeft.y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+
+        TracePoint obsptright(ptRight.x,ptRight.y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptright);
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        //int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
+                        //int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+//                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+//                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+
+                        //		DecideGps00().lidarDistance = obsPoint.y;
+                        return obsPoint;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        //int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
+                        //int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+//                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+//                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+
+                        //		DecideGps00().lidarDistance = obsPoint.y;
+                        return obsPoint;
+                    }
+                }
+            }
+
+//            if (count >= 2)
+//            {
+//                obsPoint.x = gpsTrace[j].x;
+//                obsPoint.y = gpsTrace[j].y;
+
+//                int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
+//                int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
+
+//                obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+//                obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+//                obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+//                isRemove = true;
+//                givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+//                              obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+//                //		DecideGps00().lidarDistance = obsPoint.y;
+//                return obsPoint;
+//            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}*/
+
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
+    }
+
+//    ServiceCarStatus.obsTraceLeft.clear();
+//    ServiceCarStatus.obsTraceRight.clear();
+
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+
+//        TracePoint obsptleft(ptLeft.x,ptLeft.y);
+//        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+
+//        TracePoint obsptright(ptRight.x,ptRight.y);
+//        ServiceCarStatus.obsTraceRight.push_back(obsptright);
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+        }
+    }
+    return obsPoint;
+}
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.lidarGpsXiuzheng;
+    }
+
+    bool isRemove = false;
+
+    ServiceCarStatus.obsTraceLeft.clear();
+    ServiceCarStatus.obsTraceRight.clear();
+
+    for(int j=0;j<gpsTraceLeft.size();j++)
+    {
+        TracePoint obsptleft(gpsTraceLeft[j].x,gpsTraceLeft[j].y);
+        ServiceCarStatus.obsTraceLeft.push_back(obsptleft);
+    }
+    for(int j=0;j<gpsTraceRight.size();j++)
+    {
+        TracePoint obsptright(gpsTraceRight[j].x,gpsTraceRight[j].y);
+        ServiceCarStatus.obsTraceRight.push_back(obsptright);
+    }
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>ServiceCarStatus.msysparam.lidarGpsXiuzheng)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+                        return obsPoint;
+                    }
+                }
+            }
+        }
+    }
+    return obsPoint;
+}
+
+
+
+
+
+
+//1220
+iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=0-ServiceCarStatus.msysparam.rearLidarGpsXiuzheng;
+    }
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue + PI / 2),
+                       carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * cos(anglevalue - PI / 2),
+                        carFronty + (ServiceCarStatus.msysparam.vehWidth-0.3) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y<(0-ServiceCarStatus.msysparam.rearGpsXiuzheng) )
+        {
+            int count = 0;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+                int dy = (pty + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                //		DecideGps00().lidarDistance = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        //1127 fanwei xiuzheng
+        float buchang=0;
+        Point2D ptLeft(carFrontx + (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
+                       carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
+                        carFronty +  (ServiceCarStatus.msysparam.vehWidth+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= ServiceCarStatus.msysparam.vehWidth; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / ServiceCarStatus.msysparam.vehWidth * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / ServiceCarStatus.msysparam.vehWidth * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                DecideGps00().lidarDistanceAvoid = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;
+
+    return obsPoint;
+}
+
+
+
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
+//	bool isRemove = false;
+//
+//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//	{
+//
+//		for (int i = 0; i < esrRadars.size(); i++)
+//			if ((esrRadars[i].nomal_y) != 0)
+//			{
+//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//				double yyy = esrRadars[i].nomal_y;
+//
+//				if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//				{
+//
+//					if (lastEsrID == (esrRadars[i]).esr_ID)
+//					{
+//						lastEsrCount++;
+//					}
+//					else
+//					{
+//						lastEsrCount = 0;
+//					}
+//
+//					if (lastEsrCount >= 3)
+//					{
+//						return i;
+//					}
+//
+//					lastEsrID = (esrRadars[i]).esr_ID;
+//				}
+//			}
+//	}
+//	return -1;
+//}
+
+
+
+
+int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint) {
+    bool isRemove = false;
+
+    float xiuzheng=0;
+    if(!ServiceCarStatus.useMobileEye){
+        xiuzheng=ServiceCarStatus.msysparam.radarGpsXiuzheng;
+    }
+
+    float fxiuzhengCs = DecideGps00().xiuzhengCs;
+    int nsize = gpsTrace.size();
+    for (int j = 1; j < nsize - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ xiuzheng;
+
+                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+//优化
+//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*ServiceCarStatus.msysparam.vehWidth / 2.0+DecideGps00().xiuzhengCs)){
+//                    *esrPathpoint = j;
+//                    return i;
+//                }
+
+
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+fxiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrID == i)
+                    {
+                        lastEsrCount++;
+                    }
+                    else
+                    {
+                        lastEsrCount = 0;
+                    }
+
+                    if(yyy>50 ){
+                        if (lastEsrCount >=200)
+                        {
+                            return i;
+                        }
+                    }
+                    else if (lastEsrCount >= 1)
+                    {
+                        return i;
+                    }
+
+                    lastEsrID = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+int  iv::decition::Compute00::getRearEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
+    bool isRemove = false;
+
+    float xiuzheng = ServiceCarStatus.msysparam.rearRadarGpsXiuzheng;
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_rear_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_rear_radar[i].valid))
+            {
+                double xxx = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_x + Esr_Offset);
+                double yyy = 0-(ServiceCarStatus.obs_rear_radar[i].nomal_y+ xiuzheng);
+
+                if(ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
+                    xxx=0-xxx;
+                }
+
+                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrID == i)
+                    {
+                        lastEsrCount++;
+                    }
+                    else
+                    {
+                        lastEsrCount = 0;
+                    }
+
+                    if(yyy>50 ){
+                        if (lastEsrCount >=200)
+                        {
+                            return i;
+                        }
+                    }
+                    else if (lastEsrCount >= 1)
+                    {
+                        return i;
+                    }
+
+                    lastEsrID = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
+//    bool isRemove = false;
+
+//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//    {
+
+//        for (int i = 0; i < 64; i++)
+//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
+//            {
+//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
+//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+
+//                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//                {
+
+//                    if (lastEsrID == i)
+//                    {
+//                        lastEsrCount++;
+//                    }
+//                    else
+//                    {
+//                        lastEsrCount = 0;
+//                    }
+
+//                    if(yyy>50 ){
+//                        if (lastEsrCount >=200)
+//                        {
+//                            return i;
+//                        }
+//                    }
+//                    else if (lastEsrCount >= 3)
+//                    {
+//                        return i;
+//                    }
+
+//                    lastEsrID = i;
+//                }
+//            }
+//    }
+//    return -1;
+//}
+
+
+
+int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
+
+                if (abs(xxx - gpsTrace[j].x) <= 3.0*ServiceCarStatus.msysparam.vehWidth / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrIDAvoid == i)
+                    {
+                        lastEsrCountAvoid++;
+                    }
+                    else
+                    {
+                        lastEsrCountAvoid = 0;
+                    }
+
+                    if (lastEsrCountAvoid >= 6)
+                    {
+                        return i;
+                    }
+
+                    lastEsrIDAvoid = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+
+
+
+//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
+//	double obsSpeed = 0 - realSecSpeed;
+//	double minDis = iv::MaxValue;
+//	for (int i = 0; i < esrRadars.size(); i++)
+//		if ((esrRadars[i].nomal_y) != 0)
+//		{
+//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//			double yyy = esrRadars[i].nomal_y;
+//
+//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+//			{
+//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+//				if (tmpDis < minDis)
+//				{
+//					minDis = tmpDis;
+//					obsSpeed = esrRadars[i].speed_y;
+//				}
+//			}
+//		}
+//
+//	return obsSpeed;
+//
+//
+//}
+
+
+
+
+double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+
+
+double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 10; KEPos = 8;
+        if (realSpeed > 60) KEang = 5;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
+    {
+        gpsIndex = DecideGps00().gpsLineParkIndex;
+    }
+
+
+
+    EPos = gpsTrace[gpsIndex].x + avoidX;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAvoidAveDef(farTrace, avoidX);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+
+    return ang;
+}
+
+
+std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
+
+    vector<vector<iv::GPSData>> maps;
+    vector<iv::GPSData> gpsMapLineBeside;
+    int sizeN = gpsMapLine.size();
+    for (int i = 1; i < sizeN; i++)
+    {
+        iv::GPSData gpsData(new GPS_INS);
+        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
+        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
+        double lng = ServiceCarStatus.location->ins_heading_angle;
+
+
+        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
+        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
+        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+
+        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
+
+        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
+        gpsData->gps_x = x0 + k1 * avoidX;
+        gpsData->gps_y = y0 + k2 * avoidX;
+        gpsMapLineBeside.push_back(gpsData);
+
+    }
+    return gpsMapLineBeside;
+
+}
+
+
+
+//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
+
+//    double ang = 0;
+//    double EPos = 0, EAng = 0;
+
+// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
+//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
+
+// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+//    double PreviewDistance;//预瞄距离
+//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
+////        if (realSpeed > 50) KEang = 5;
+
+
+
+
+
+//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+//double a = ServiceCarStatus.Lane.curvature;
+//double b = ServiceCarStatus.Lane.heading;
+//double c = (c1+c2)*0.5;
+//double x= PreviewDistance;
+//double y;
+
+
+//y=a*x*x+b*x+c;
+
+//   // EPos = y;
+//EPos=c;
+
+
+//  //  EAng=atan(2*a*x+b) / PI * 180;
+//    EAng=ServiceCarStatus.Lane.yaw;
+//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+//        lastEA = EAng;
+//        lastEP = EPos;
+
+
+//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
+//            std::cout << "\nEAng:%f\n" << EAng << std::endl;
+
+
+//        if (ang > angleLimit) {
+//            ang = angleLimit;
+//        }
+//        else if (ang < -angleLimit) {
+//            ang = -angleLimit;
+//        }
+//        if (lastAng != iv::MaxValue) {
+//            ang = 0.2 * lastAng + 0.8 * ang;
+//            //ODS("lastAng：%d\n", lastAng);
+//        }
+//        lastAng = ang;
+//        return ang;
+//    }
+
+
+
+double IEPos = 0, IEang = 0;
+
+
+double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+    double Curve=0;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+    double KCurve=120;
+    double KIEPos = 0, KIEang = 0;
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+
+    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
+    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
+    int conf =min(confL,confR);
+
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    KEPos = 20;
+    KEang = 200;
+    //KEang = 15;
+
+    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+    double a = ServiceCarStatus.Lane.curvature;
+    double b = ServiceCarStatus.Lane.heading;
+    double c = (c1+c2)*0.5;
+    double yaw= ServiceCarStatus.Lane.yaw;
+
+    double x= PreviewDistance;
+    double y;
+
+
+    y=c-(a*x*x+b*x);
+    double difa=0-(atan(2*a*x+b) / PI * 180);
+    Curve=0-a;
+
+    //EAng=difa;
+    //EPos=y;
+    EAng= 0-b;
+    EPos = c;
+    DEang = 10;
+    DEPos = 20;
+    //DEang = 20;
+    //DEPos = 10;
+
+    IEang = EAng+0.7*IEang;
+    IEPos = EPos+0.7*IEPos;
+    KIEang = 0;
+    //KIEang = 0.5;
+    KIEPos =2;
+
+
+
+    if(abs(confL)>=2&&abs(confR)>=2){
+        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+    }else{
+        ang=lastAng;
+    }
+    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+double iv::decition::Compute00::bocheCompute(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 x_1 = pt.x;
+    double y_1 = pt.y;
+    double angle_1 = getQieXianAngle(nowGps,aimGps);
+    double x_2 = 0.0, y_2 = 0.0;
+    double steering_angle;
+    double l = 2.950;
+    double r =6;
+    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double g_1 = tan(angle_1);
+    double car_pos[3] = { x_1,y_1,g_1 };
+    double parking_pos[2] = { x_2,y_2 };
+    double g_3;
+    double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+
+    //g_3 = 0 - 0.5775;
+    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
+    //交点
+    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
+    y_3 = y_1 - g_1 * x_1;
+    //圆心1
+    x_o_1 = r;
+    y_o_1 = g_3 * r + y_3;
+    //圆形1的切点1
+    x_t_1 = 0.0;
+    y_t_1 = g_3 * r + y_3;
+    //圆形1的切点2
+    if (g_1 == 0)
+    {
+        x_t_2 = r;
+        y_t_2 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    //圆心2
+    x_o_2 = 0 - r;
+    y_o_2 = y_3 - g_3 * r;
+    //圆形2的切点1
+    x_t_3 = 0;
+    y_t_3 = y_3 - g_3 * r;
+    //圆形2的切点2
+    if (g_1 == 0)
+    {
+        x_t_4 = 0 - r;
+        y_t_4 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            x_t_n = x_t_1;
+            y_t_n = y_t_1;
+            x_t_f = x_t_2;
+            y_t_f = y_t_2;
+        }
+        else
+        {
+            x_t_n = x_t_2;
+            y_t_n = y_t_2;
+            x_t_f = x_t_1;
+            y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            x_t_n = x_t_3;
+            y_t_n = y_t_3;
+            x_t_f = x_t_4;
+            y_t_f = y_t_4;
+        }
+        else
+        {
+            x_t_n = x_t_4;
+            y_t_n = y_t_4;
+            x_t_f = x_t_3;
+            y_t_f = y_t_3;
+        }
+
+
+
+
+    }
+    steering_angle = atan2(l, r);
+
+    if (x_t_n < 0)
+    {
+        steering_angle = 0 - steering_angle;
+    }
+
+    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    bocheAngle = steering_angle*180/PI;
+
+    cout << "近切点：x_t_n=" << x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    cout << "航向角：" << steering_angle << endl;
+
+
+    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
+    //        return 1;
+    //    }
+    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
+    double disA = GetDistance(aimGps,nowGps);
+    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
+        return 1;
+    }
+
+    return 0;
+
+}
+
+
+
+//返回垂直平分线的斜率
+double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
+    double angl, x_3, angle_3;
+    if (tan(angle_1 == 0))
+    {
+        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
+        {
+            angle_3 = 0 - 1;
+        }
+        else
+        {
+            angle_3 = 1;
+        }
+    }
+    else
+    {
+        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
+        angl = tan(angle_1);//车所在直线的斜率
+        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
+        {
+            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+
+
+            }
+
+        }
+        else//第二象限
+        {
+            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+            }
+        }
+    }
+
+    return angle_3;
+
+}
+
+
+
+double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
+    double heading = nowGps.ins_heading_angle *PI/180;
+    double x1 = nowGps.gps_x;
+    double y1 = nowGps.gps_y;
+    if (heading<=PI*0.5)
+    {
+        heading = 0.5*PI - heading;
+    }
+    else if (heading>PI*0.5 && heading<=PI*1.5) {
+        heading = 1.5*PI - heading;
+    }
+    else if (heading>PI*1.5) {
+        heading = 2.5*PI - heading;
+    }
+    double k1 = tan(heading);
+    double x = x1+10;
+    double y = k1 * x + y1 - (k1 * x1);
+    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
+    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
+    double angle = atan(abs(xielv));
+    if (xielv<0)
+    {
+        angle = PI - angle;
+    }
+    return angle;
+
+}
+
+
+/*
+  chuizhicheweiboche
+  */
+
+
+int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
+{
+
+
+
+    double l=2.95;//轴距
+    double x_0 = 0, y_0 = 0.5;
+    double x_1, y_1;//车起点坐标
+    double ange1;//车航向角弧度
+    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
+    double real_rad;;//另一条直线的航向角弧度
+    double angle_3;//垂直平分线弧度
+    double x_3, y_3;//垂直平分线交点
+    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
+    double x_o_1, y_o_1;//圆形1坐标
+    double x_o_2, y_o_2;//圆形2坐标
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double min_rad;
+    double R_M; //后轴中点的转弯半径
+    double steering_angle;
+
+
+
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    x_1=pt.x;
+    y_1=pt.y;
+    ange1=getQieXianAngle(nowGps,aimGps);
+
+    min_rad_zhuanxiang(&R_M , &min_rad);
+    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
+    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
+    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
+    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
+    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
+                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
+    steering_angle = atan2(l, R_M);
+    x_4 = 0.5;
+    y_4 = 0;
+    //for (int i = 0; i < 4; i++)
+    //{
+    //for (int j = 0; j < 2; j++)
+    //{
+    //	cout << t[i][j] << endl;
+    //}
+    //}
+    //cout << "min_rad:" << min_rad<< endl;
+    //cout << "jiaodian：x=" << x_3 << endl;
+    //cout << "jiaodian：y=" << y_3 << endl;
+    // cout << "R-M：" << R_M << endl;
+    cout << "x_0:" << x_0 << endl;
+    cout << "y_0:" << y_0 << endl;
+    cout << "x_2:" << x_2 << endl;
+    cout << "y_2:" << y_2 << endl;
+    cout << "近切点：x_t_n="<< x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    //cout << "航向角：" << steering_angle << endl;
+    //cout << "圆心1横坐标=" << x_o_1 << endl;
+    //cout << "圆心1纵坐标=" << y_o_1 << endl;
+    //cout << "圆心2横坐标=" << x_o_2 << endl;
+    //cout << "圆心2纵坐标=" << y_o_2 << endl;
+    //cout << "平分线弧度=" << angle_3 << endl;
+    //cout << " min_rad=" << min_rad << endl;
+    //cout << " real_rad=" << real_rad << endl;
+    //   system("PAUSE");
+
+
+
+    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
+    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
+    dBocheAngle = steering_angle*180/PI;
+
+    double disA = GetDistance(aimGps,nowGps);
+
+    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
+        return 1;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
+    double L_c = 4.749;//车长
+    double rad_1;
+    double rad_2;
+    double L_k = 1.931;//车宽
+    double L = 2.95;//轴距
+    double L_f =1.2 ;//前悬
+    double L_r =0.7 ;//后悬
+    double R_min =6.5 ;//最小转弯半径
+    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
+    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
+    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
+    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
+    return 0;
+}
+
+
+double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
+
+    if (x_1 > 0 && y_1 > 0)
+    {
+        *real_rad = PI*0.5 - min_rad;
+        *x_2 = R_M - R_M*cos(min_rad);
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    else
+    {
+        *real_rad = PI*0.5 + min_rad;
+        *x_2 = R_M*cos(min_rad) - R_M;
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
+    double b1, b2;
+    double k1, k2;
+    if (ange1!=(PI*0.5))
+    {
+        k1 = tan(ange1);
+        b1 = y_1 - k1*x_1;
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = (b2 - b1) / (k1 - k2);
+        *y_3 = k2*(*x_3) + b2;
+    }
+    else
+    {
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = x_1;
+        *y_3 = k2*(*x_3) + b2;
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
+    double k1, k2;
+    double  angle_j;
+    k2 = tan(real_rad);
+    if (ange1 != (PI*0.5))
+    {
+        k1 = tan(ange1);
+        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
+
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    else
+    {
+        angle_j = min_rad;//两直线夹角
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
+    double b2, b3, k2, k3;
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(angle_3)*x_3;
+    k2 = tan(real_rad);
+    k3 = tan(angle_3);
+    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
+    *y_o_1 = k3*(*x_o_1) + b3;
+
+    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
+    *y_o_2 = k3*(*x_o_2) + b3;
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
+{
+    double x_o, y_o;
+    double b2, b3, k1, k2, k3;
+    //double car_pos[3] = { x_1,y_1,k1 };
+    double parking_pos[2] = { x_2,y_2 };
+    //double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double t[4][2];
+    k1 = tan(ange1);
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(real_rad)*x_3;
+    k2 = tan(real_rad);//另一条直线斜率
+    k3 = tan(angle_3);//垂直平分线斜率
+    //圆心1和2切点*********************************************
+    if (x_1 > 0 && y_1 > 0)//第一象限
+    {
+        if (ange1 == (PI*0.5))
+        {
+            x_t_1 = x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+    }
+    else
+    {
+        if (ange1 == 0)
+        {
+            x_t_1 = 0 - x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = 0 - x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+
+    }
+
+    //圆心1和2切点*********************************************
+
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            *x_t_n = x_t_1;
+            *y_t_n = y_t_1;
+            *x_t_f = x_t_2;
+            *y_t_f = y_t_2;
+        }
+        else
+        {
+            *x_t_n = x_t_2;
+            *y_t_n = y_t_2;
+            *x_t_f = x_t_1;
+            *y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            *x_t_n = x_t_3;
+            *y_t_n = y_t_3;
+            *x_t_f = x_t_4;
+            *y_t_f = y_t_4;
+        }
+        else
+        {
+            *x_t_n = x_t_4;
+            *y_t_n = y_t_4;
+            *x_t_f = x_t_3;
+            *y_t_f = y_t_3;
+        }
+
+
+
+    }
+
+    return 0;
+
+}
+
+
+int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
+{
+    int index = -1;
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+    float minDis=20;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis=tmpdis;
+        }
+    }
+    return index;
+}
+
+
+double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    FrenetPoint esr_obs_F_point;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
+                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
+//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
+                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
+                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
+                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
+
+                    obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+int iv::decition::Compute00::getEsrIndexByFrenet(std::vector<Point2D> gpsTrace, FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps){
+    double minDistance = numeric_limits<double>::max();
+    int minDis_index=-1;
+
+    for(int i=0; i<64; ++i){
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
+            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+            //将毫米波障碍物位置转换到frenet坐标系下
+//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
+            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
+
+            //如果障碍物与道路的横向距离d<=3.0*ServiceCarStatus.msysparam.vehWidth / 4.0，则认为道路上有障碍物。
+            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
+            //minDistance、minDis_index用来统计最近的障碍物信息。
+            if(abs(esrObsPoint.d)<=(3.0*ServiceCarStatus.msysparam.vehWidth / 4.0+DecideGps00().xiuzhengCs)){
+                if(esrObsPoint.s<minDistance){
+                    minDistance = esrObsPoint.s;
+                    minDis_index = i;
+                }
+            }
+        }
+    }
+    return minDis_index;
+}
+
+
+
+
+std::vector<std::vector<iv::GPSData>> gmapsL;
+std::vector<std::vector<iv::GPSData>> gmapsR;

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/compute_00.h

@@ -0,0 +1,106 @@
+#pragma once
+#ifndef _IV_DECITION_COMPUTE_00_
+#define _IV_DECITION_COMPUTE_00_
+
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+#include <decition/decide_gps_00.h>
+
+namespace iv {
+    namespace decition {
+    //根据传感器传输来的信息作出决策
+        class Compute00 {
+        public:
+            Compute00();
+            ~Compute00();
+
+            static   double maxAngle;
+            static	 double angleLimit;  //角度限制
+            static	 double lastEA;      //上一次角度误差
+            static	 double lastEP;      //上一次位置误差
+            static	 double decision_Angle;  //决策角度
+            static	 double lastAng;         //上次角度
+            static   int lastEsrID;          //上次毫米波障碍物ID
+            static   int  lastEsrCount;      //毫米波障碍物累计次数
+
+            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
+            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
+
+            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
+            static double bocheAngle,dBocheAngle;
+
+            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);
+
+            static int getDesiredSpeed(std::vector<GPSData> gpsMap);
+            static int getMapDelta(std::vector<GPSData> MapTrace);
+            static int getPlanSpeed(std::vector<GPSData> MapTrace);
+
+
+            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
+            static double getAveDef(std::vector<Point2D> farTrace);
+            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
+            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static Point2D getLidarObsPointXY(std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight, iv::LidarGridPtr lidarGridPtr);
+
+
+
+            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint);
+            static int getRearEsrIndex(std::vector<Point2D> gpsTrace , int roadNum);
+            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);
+
+            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);
+
+            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
+            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX);
+
+            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);
+
+            static double getDecideAngleByLane(double realSpeed);
+
+            static double getDecideAngleByLanePID(double realSpeed);
+
+            static double bocheCompute(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);
+
+            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
+
+            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
+            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
+                                    double *x_2, double *y_2, double *real_rad);
+            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
+                                                double ange1,double real_rad,double *x_3, double *y_3);
+            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
+                                                   double min_rad,double *angle_3);
+            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
+                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
+            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
+                                          double real_rad,double angle_3,double R_M,
+                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);
+
+            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);
+
+            static int getEsrIndexByFrenet(std::vector<Point2D> gpsTrace,FrenetPoint &esrObsPoint, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);
+            double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D> gpsTrace, std::vector<iv::GPSData> gpsMap,int pathpoint,GPS_INS nowGps);
+
+        private:
+
+        };
+    }
+}
+extern std::vector<std::vector<iv::GPSData>> gmapsL;
+extern std::vector<std::vector<iv::GPSData>> gmapsR;
+
+extern std::vector<int> lastEsrIdVector;
+extern std::vector<int> lastEsrCountVector;
+
+#endif // !_IV_DECITION_COMPUTE_00_
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/dubins.cpp

@@ -0,0 +1,439 @@
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifdef WIN32
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+#include "dubins.h"
+
+#define EPSILON (10e-10)
+
+typedef enum
+{
+    L_SEG = 0,
+    S_SEG = 1,
+    R_SEG = 2
+} SegmentType;
+
+/* The segment types for each of the Path types */
+const SegmentType DIRDATA[][3] = {
+    { L_SEG, S_SEG, L_SEG },
+    { L_SEG, S_SEG, R_SEG },
+    { R_SEG, S_SEG, L_SEG },
+    { R_SEG, S_SEG, R_SEG },
+    { R_SEG, L_SEG, R_SEG },
+    { L_SEG, R_SEG, L_SEG }
+};
+
+typedef struct
+{
+    double alpha;
+    double beta;
+    double d;
+    double sa;
+    double sb;
+    double ca;
+    double cb;
+    double c_ab;
+    double d_sq;
+} DubinsIntermediateResults;
+
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
+
+/**
+ * Floating point modulus suitable for rings
+ *
+ * fmod doesn't behave correctly for angular quantities, this function does
+ */
+double fmodr( double x, double y)
+{
+    return x - y*floor(x/y);
+}
+
+double mod2pi( double theta )
+{
+    return fmodr( theta, 2 * M_PI );
+}
+
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
+{
+    int i, errcode;
+    DubinsIntermediateResults in;
+    double params[3];
+    double cost;
+    double best_cost = INFINITY;
+    int best_word = -1;
+    errcode = dubins_intermediate_results(&in, q0, q1, rho);
+    if(errcode != EDUBOK) {
+        return errcode;
+    }
+
+
+    path->qi[0] = q0[0];
+    path->qi[1] = q0[1];
+    path->qi[2] = q0[2];
+    path->rho = rho;
+
+    for( i = 0; i < 6; i++ ) {
+        DubinsPathType pathType = (DubinsPathType)i;
+        errcode = dubins_word(&in, pathType, params);
+        if(errcode == EDUBOK) {
+            cost = params[0] + params[1] + params[2];
+            if(cost < best_cost) {
+                best_word = i;
+                best_cost = cost;
+                path->param[0] = params[0];
+                path->param[1] = params[1];
+                path->param[2] = params[2];
+                path->type = pathType;
+            }
+        }
+    }
+    if(best_word == -1) {
+        return EDUBNOPATH;
+    }
+    return EDUBOK;
+}
+
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
+{
+    int errcode;
+    DubinsIntermediateResults in;
+    errcode = dubins_intermediate_results(&in, q0, q1, rho);
+    if(errcode == EDUBOK) {
+        double params[3];
+        errcode = dubins_word(&in, pathType, params);
+        if(errcode == EDUBOK) {
+            path->param[0] = params[0];
+            path->param[1] = params[1];
+            path->param[2] = params[2];
+            path->qi[0] = q0[0];
+            path->qi[1] = q0[1];
+            path->qi[2] = q0[2];
+            path->rho = rho;
+            path->type = pathType;
+        }
+    }
+    return errcode;
+}
+
+double dubins_path_length( DubinsPath* path )
+{
+    double length = 0.;
+    length += path->param[0];
+    length += path->param[1];
+    length += path->param[2];
+    length = length * path->rho;
+    return length;
+}
+
+double dubins_segment_length( DubinsPath* path, int i )
+{
+    if( (i < 0) || (i > 2) )
+    {
+        return INFINITY;
+    }
+    return path->param[i] * path->rho;
+}
+
+double dubins_segment_length_normalized( DubinsPath* path, int i )
+{
+    if( (i < 0) || (i > 2) )
+    {
+        return INFINITY;
+    }
+    return path->param[i];
+}
+
+DubinsPathType dubins_path_type( DubinsPath* path )
+{
+    return path->type;
+}
+
+void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
+{
+    double st = sin(qi[2]);
+    double ct = cos(qi[2]);
+    if( type == L_SEG ) {
+        qt[0] = +sin(qi[2]+t) - st;
+        qt[1] = -cos(qi[2]+t) + ct;
+        qt[2] = t;
+    }
+    else if( type == R_SEG ) {
+        qt[0] = -sin(qi[2]-t) + st;
+        qt[1] = +cos(qi[2]-t) - ct;
+        qt[2] = -t;
+    }
+    else if( type == S_SEG ) {
+        qt[0] = ct * t;
+        qt[1] = st * t;
+        qt[2] = 0.0;
+    }
+    qt[0] += qi[0];
+    qt[1] += qi[1];
+    qt[2] += qi[2];
+}
+
+int dubins_path_sample( DubinsPath* path, double t, double q[3] )
+{
+    /* tprime is the normalised variant of the parameter t */
+    double tprime = t / path->rho;
+    double qi[3]; /* The translated initial configuration */
+    double q1[3]; /* end-of segment 1 */
+    double q2[3]; /* end-of segment 2 */
+    const SegmentType* types = DIRDATA[path->type];
+    double p1, p2;
+
+    if( t < 0 || t > dubins_path_length(path) ) {
+        return EDUBPARAM;
+    }
+
+    /* initial configuration */
+    qi[0] = 0.0;
+    qi[1] = 0.0;
+    qi[2] = path->qi[2];
+
+    /* generate the target configuration */
+    p1 = path->param[0];
+    p2 = path->param[1];
+    dubins_segment( p1,      qi,    q1, types[0] );
+    dubins_segment( p2,      q1,    q2, types[1] );
+    if( tprime < p1 ) {
+        dubins_segment( tprime, qi, q, types[0] );
+    }
+    else if( tprime < (p1+p2) ) {
+        dubins_segment( tprime-p1, q1, q,  types[1] );
+    }
+    else {
+        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
+    }
+
+    /* scale the target configuration, translate back to the original starting point */
+    q[0] = q[0] * path->rho + path->qi[0];
+    q[1] = q[1] * path->rho + path->qi[1];
+    q[2] = mod2pi(q[2]);
+
+    return EDUBOK;
+}
+
+int dubins_path_sample_many(DubinsPath* path, double stepSize,
+                            DubinsPathSamplingCallback cb, void* user_data)
+{
+    int retcode;
+    double q[3];
+    double x = 0.0;
+    double length = dubins_path_length(path);
+    while( x <  length ) {
+        dubins_path_sample( path, x, q );
+        retcode = cb(q, x, user_data);
+        if( retcode != 0 ) {
+            return retcode;
+        }
+        x += stepSize;
+    }
+    return 0;
+}
+
+int dubins_path_endpoint( DubinsPath* path, double q[3] )
+{
+    return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
+}
+
+int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
+{
+    /* calculate the true parameter */
+    double tprime = t / path->rho;
+
+    if((t < 0) || (t > dubins_path_length(path)))
+    {
+        return EDUBPARAM;
+    }
+
+    /* copy most of the data */
+    newpath->qi[0] = path->qi[0];
+    newpath->qi[1] = path->qi[1];
+    newpath->qi[2] = path->qi[2];
+    newpath->rho   = path->rho;
+    newpath->type  = path->type;
+
+    /* fix the parameters */
+    newpath->param[0] = fmin( path->param[0], tprime );
+    newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
+    newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
+    return 0;
+}
+
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
+{
+    double dx, dy, D, d, theta, alpha, beta;
+    if( rho <= 0.0 ) {
+        return EDUBBADRHO;
+    }
+
+    dx = q1[0] - q0[0];
+    dy = q1[1] - q0[1];
+    D = sqrt( dx * dx + dy * dy );
+    d = D / rho;
+    theta = 0;
+
+    /* test required to prevent domain errors if dx=0 and dy=0 */
+    if(d > 0) {
+        theta = mod2pi(atan2( dy, dx ));
+    }
+    alpha = mod2pi(q0[2] - theta);
+    beta  = mod2pi(q1[2] - theta);
+
+    in->alpha = alpha;
+    in->beta  = beta;
+    in->d     = d;
+    in->sa    = sin(alpha);
+    in->sb    = sin(beta);
+    in->ca    = cos(alpha);
+    in->cb    = cos(beta);
+    in->c_ab  = cos(alpha - beta);
+    in->d_sq  = d * d;
+
+    return EDUBOK;
+}
+
+int dubins_LSL(DubinsIntermediateResults* in, double out[3])
+{
+    double tmp0, tmp1, p_sq;
+
+    tmp0 = in->d + in->sa - in->sb;
+    p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
+
+    if(p_sq >= 0) {
+        tmp1 = atan2( (in->cb - in->ca), tmp0 );
+        out[0] = mod2pi(tmp1 - in->alpha);
+        out[1] = sqrt(p_sq);
+        out[2] = mod2pi(in->beta - tmp1);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+
+int dubins_RSR(DubinsIntermediateResults* in, double out[3])
+{
+    double tmp0 = in->d - in->sa + in->sb;
+    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
+    if( p_sq >= 0 ) {
+        double tmp1 = atan2( (in->ca - in->cb), tmp0 );
+        out[0] = mod2pi(in->alpha - tmp1);
+        out[1] = sqrt(p_sq);
+        out[2] = mod2pi(tmp1 -in->beta);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_LSR(DubinsIntermediateResults* in, double out[3])
+{
+    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
+    if( p_sq >= 0 ) {
+        double p    = sqrt(p_sq);
+        double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
+        out[0] = mod2pi(tmp0 - in->alpha);
+        out[1] = p;
+        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_RSL(DubinsIntermediateResults* in, double out[3])
+{
+    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
+    if( p_sq >= 0 ) {
+        double p    = sqrt(p_sq);
+        double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
+        out[0] = mod2pi(in->alpha - tmp0);
+        out[1] = p;
+        out[2] = mod2pi(in->beta - tmp0);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_RLR(DubinsIntermediateResults* in, double out[3])
+{
+    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
+    double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
+    if( fabs(tmp0) <= 1) {
+        double p = mod2pi((2*M_PI) - acos(tmp0) );
+        double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
+        out[0] = t;
+        out[1] = p;
+        out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_LRL(DubinsIntermediateResults* in, double out[3])
+{
+    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
+    double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
+    if( fabs(tmp0) <= 1) {
+        double p = mod2pi( 2*M_PI - acos( tmp0) );
+        double t = mod2pi(-in->alpha - phi + p/2.);
+        out[0] = t;
+        out[1] = p;
+        out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3])
+{
+    int result;
+    switch(pathType)
+    {
+    case LSL:
+        result = dubins_LSL(in, out);
+        break;
+    case RSL:
+        result = dubins_RSL(in, out);
+        break;
+    case LSR:
+        result = dubins_LSR(in, out);
+        break;
+    case RSR:
+        result = dubins_RSR(in, out);
+        break;
+    case LRL:
+        result = dubins_LRL(in, out);
+        break;
+    case RLR:
+        result = dubins_RLR(in, out);
+        break;
+    default:
+        result = EDUBNOPATH;
+    }
+    return result;
+}
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/dubins.h

@@ -0,0 +1,149 @@
+#ifndef DUBINS_H
+#define DUBINS_H
+
+typedef enum
+{
+    LSL = 0,
+    LSR = 1,
+    RSL = 2,
+    RSR = 3,
+    RLR = 4,
+    LRL = 5
+} DubinsPathType;
+
+typedef struct
+{
+    /* the initial configuration */
+    double qi[3];
+    /* the lengths of the three segments */
+    double param[3];
+    /* model forward velocity / model angular velocity */
+    double rho;
+    /* the path type described */
+    DubinsPathType type;
+} DubinsPath;
+
+#define EDUBOK        (0)   /* No error */
+#define EDUBCOCONFIGS (1)   /* Colocated configurations */
+#define EDUBPARAM     (2)   /* Path parameterisitation error */
+#define EDUBBADRHO    (3)   /* the rho value is invalid */
+#define EDUBNOPATH    (4)   /* no connection between configurations with this word */
+
+/**
+ * Callback function for path sampling
+ *
+ * @note the q parameter is a configuration
+ * @note the t parameter is the distance along the path
+ * @note the user_data parameter is forwarded from the caller
+ * @note return non-zero to denote sampling should be stopped
+ */
+typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
+
+/**
+ * Generate a path from an initial configuration to
+ * a target configuration, with a specified maximum turning
+ * radii
+ *
+ * A configuration is (x, y, theta), where theta is in radians, with zero
+ * along the line x = 0, and counter-clockwise is positive
+ *
+ * @param path  - the resultant path
+ * @param q0    - a configuration specified as an array of x, y, theta
+ * @param q1    - a configuration specified as an array of x, y, theta
+ * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @return      - non-zero on error
+ */
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
+
+/**
+ * Generate a path with a specified word from an initial configuration to
+ * a target configuration, with a specified turning radius
+ *
+ * @param path     - the resultant path
+ * @param q0       - a configuration specified as an array of x, y, theta
+ * @param q1       - a configuration specified as an array of x, y, theta
+ * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @param pathType - the specific path type to use
+ * @return         - non-zero on error
+ */
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
+
+/**
+ * Calculate the length of an initialised path
+ *
+ * @param path - the path to find the length of
+ */
+double dubins_path_length(DubinsPath* path);
+
+/**
+ * Return the length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length(DubinsPath* path, int i);
+
+/**
+ * Return the normalized length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length_normalized( DubinsPath* path, int i );
+
+/**
+ * Extract an integer that represents which path type was used
+ *
+ * @param path    - an initialised path
+ * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL
+ */
+DubinsPathType dubins_path_type(DubinsPath* path);
+
+/**
+ * Calculate the configuration along the path, using the parameter t
+ *
+ * @param path - an initialised path
+ * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
+ * @param q    - the configuration result
+ * @returns    - non-zero if 't' is not in the correct range
+ */
+int dubins_path_sample(DubinsPath* path, double t, double q[3]);
+
+/**
+ * Walk along the path at a fixed sampling interval, calling the
+ * callback function at each interval
+ *
+ * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
+ *
+ * @param path      - the path to sample
+ * @param stepSize  - the distance along the path for subsequent samples
+ * @param cb        - the callback function to call for each sample
+ * @param user_data - optional information to pass on to the callback
+ *
+ * @returns - zero on successful completion, or the result of the callback
+ */
+int dubins_path_sample_many(DubinsPath* path,
+                            double stepSize,
+                            DubinsPathSamplingCallback cb,
+                            void* user_data);
+
+/**
+ * Convenience function to identify the endpoint of a path
+ *
+ * @param path - an initialised path
+ * @param q    - the configuration result
+ */
+int dubins_path_endpoint(DubinsPath* path, double q[3]);
+
+/**
+ * Convenience function to extract a subset of a path
+ *
+ * @param path    - an initialised path
+ * @param t       - a length measure, where 0 < t < dubins_path_length(path)
+ * @param newpath - the resultant path
+ */
+int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
+
+
+
+#endif // DUBINS_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/gnss_coordinate_convert.cpp

@@ -0,0 +1,154 @@
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+/*
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void GaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+*/
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/gnss_coordinate_convert.h

@@ -0,0 +1,26 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/gps_distance.cpp

@@ -0,0 +1,45 @@
+#include <decition/adc_tools/gps_distance.h>
+#include <math.h>
+#define M_PI (3.1415926535897932384626433832795)
+
+// 计算弧度
+double iv::decition::rad(double d)
+{
+    const double PI = 3.1415926535898;
+    return d * PI / 180.0;
+}
+
+// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
+{
+    const float EARTH_RADIUS = 6378.137;
+    double radLat1 = rad(fLati1);
+    double radLat2 = rad(fLati2);
+    double a = radLat1 - radLat2;
+
+    double b = rad(fLong1) - rad(fLong2);
+    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
+    s = s * EARTH_RADIUS;
+    //	s = (int)(s * 10000000) / 10000;
+    return s;
+}
+
+
+// 从直角坐标两点的直线距离，单位是米
+double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
+{
+    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
+    return s;
+}
+
+
+// 从直角坐标计算两点的夹角
+double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
+{
+    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
+    return angle;
+}
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/gps_distance.h

@@ -0,0 +1,26 @@
+#pragma once
+#ifndef _IV_DECITION_GPS_DISTANCE_
+#define _IV_DECITION_GPS_DISTANCE_
+
+namespace iv {
+	namespace decition {
+        // 计算弧度
+		double rad(double d);
+
+		// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+		double CalcDistance(double , double , double , double );
+
+		//计算直角坐标距离
+		double DirectDistance(double, double, double, double);
+
+		//计算直角坐标角度
+		double DirectAngle(double, double, double, double);
+
+
+	}
+}
+
+
+
+
+#endif // !_IV_DECITION_GPS_DISTANCE_

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/obs_predict.cpp

@@ -0,0 +1,116 @@
+#include <decition/obs_predict.h>
+#include <decition/decition_type.h>
+#include <common/gps_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <decition/transfer.h>
+#include <perception/perceptionoutput.h>
+#include<common/constants.h>
+
+using namespace std;
+#define PI (3.1415926535897932384626433832795)
+
+
+double iv::decition::PredictObsDistance( double realSpeed,std::vector<Point2D>  gpsTrace,std::vector<iv::Perception::PerceptionOutput> lidar_per){
+
+  double crashDistance=200;
+    for(int i=0;i<lidar_per.size();i++){
+        if(lidar_per[i].life>300 && (lidar_per[i].velocity.x!=0 || lidar_per[i].velocity.y!=0)){
+            if(lidar_per[i].location.y>20 && lidar_per[i].velocity.y>0){
+
+            }
+            else if(lidar_per[i].location.y<0 && lidar_per[i].velocity.y<0){
+
+            }
+            else if(lidar_per[i].location.x>20 && lidar_per[i].velocity.x>0){
+
+            }else if(lidar_per[i].location.x<-20 && lidar_per[i].velocity.x<0){
+
+            }else{
+
+             double    crashDis = getCrashDis(realSpeed,gpsTrace,lidar_per[i]);
+
+                crashDistance=min(crashDis,crashDistance);
+            }
+
+
+        }
+
+    }
+
+
+
+
+
+return crashDistance;
+
+
+}
+
+double iv::decition::getCrashDis(double realSpeed,std::vector<Point2D>  gpsTrace,iv::Perception::PerceptionOutput obs){
+    double dis=0;
+    int size = gpsTrace.size()/10;
+    for(int i=1; i<size;i++){
+      double time =  getTime( realSpeed,  gpsTrace, i*10,&dis);
+      double obsX= obs.location.x+obs.velocity.x*time;
+      double obsY= obs.location.y+obs.velocity.y*time;
+      Point2D obsP(obsX,obsY);
+     double obsDis= GetDistance(obsP,gpsTrace[i*10]);
+     if(obsDis<0.7*(Veh_Width+obs.size.x) ){
+         return dis;
+     }
+
+    }
+
+    return 200;
+
+}
+
+
+
+int iv::decition::getFrameCount(double realSpeed,std::vector<Point2D>  gpsTrace){
+    double lenth=0;
+    for(int i =0 ; i<gpsTrace.size()-1;i++){
+        lenth +=GetDistance(gpsTrace[i],gpsTrace[i+1]);
+    }
+
+    int frame= lenth/realSpeed;
+    frame = min(frame,50);
+
+    return frame;
+}
+
+int  iv::decition::getPoiIndex(double realSpeed,std::vector<Point2D>  gpsTrace,int frame){
+
+
+
+        double d =realSpeed*0.1*frame;
+        int index=0;
+
+        for(int j=0;j<gpsTrace.size()-1;j++){
+            double dis;
+            dis+=GetDistance(gpsTrace[j],gpsTrace[j+1]);
+            if(dis>d){
+                index=j;
+                break;
+            }
+            index=j;
+        }
+
+        return index;
+}
+
+double iv::decition::getTime(double realSpeed,std::vector<Point2D>  gpsTrace,int frame ,double *dis){
+    int size=gpsTrace.size()-1;
+    int f=min(frame,size);
+
+    for(int i=0;i<=f;i++){
+
+        *dis+=GetDistance(gpsTrace[0],gpsTrace[f]);
+
+    }
+    double time = *dis/realSpeed;
+    return time;
+
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/obs_predict.h

@@ -0,0 +1,34 @@
+#ifndef OBS_PREDICT_H
+#define OBS_PREDICT_H
+
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+#include <decition/decition_type.h>
+#include<vector>
+#include <decition/decide_gps_00.h>
+#include <perception/perceptionoutput.h>
+namespace iv {
+    namespace decition {
+        //根据传感器传输来的信息作出决策
+
+
+
+double PredictObsDistance(double realSpeed,std::vector<Point2D>  gpsTrace,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+
+double getCrashDis(double realSpeed,std::vector<Point2D>  gpsTrace,iv::Perception::PerceptionOutput obs);
+
+int getFrameCount(double realSpeed,std::vector<Point2D>  gpsTrace);
+
+int getPoiIndex(double realSpeed,std::vector<Point2D>  gpsTrace , int frame);
+
+double getTime(double realSpeed,std::vector<Point2D>  gpsTrace , int frame ,double *dis );
+
+
+    }
+
+
+}
+
+
+
+#endif // OBS_PREDICT_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/parameter_status.h

@@ -0,0 +1,109 @@
+#ifndef PARAMETER_STATUS_H
+#define PARAMETER_STATUS_H
+
+
+
+
+#include <common/boost.h>
+#include <cstdint>
+#include <boost/serialization/singleton.hpp>
+#include <control/vv7.h>
+
+namespace iv {
+    namespace decition {
+        class ParameterStatus : public boost::noncopyable
+        {
+        public:
+            /*****************
+             * **** speed control *****
+             * ***************/
+
+
+
+
+                    float speed_kp=0.5;
+                    float speed_kd=0.3;
+                    float speed_ki=0;
+
+                    float speed_kp_t=10;
+                    float speed_kd_t=0;
+                    float speed_ki_t=0;
+
+                    float speed_increase_limite_switch=1;
+                    float speed_decline_limite_switch=1;
+                    float speed_max_increase=5;
+                    float speed_max_decline=10;
+
+
+
+
+            /*****************
+             * **** ttc control *****
+             * ***************/
+
+                float ttc_emc=0.8;
+                float ttc_urgent=1.6;
+                float ttc_early=10;
+                float brakeIntMax_emc=10;
+                float brakeIntMax_urgent=5;
+                float brakeIntMax_early=3;
+
+
+            /*****************
+             * **** wheel control *****
+             * ***************/
+
+                float wheel_p_eang=14;
+                float wheel_p_epos=10;
+                float wheel_d_eang=5;
+                float wheel_d_epos=5;
+                float wheel_i_eang=0;
+                float wheel_i_epos=0;
+
+                float  wheel_previewDistance_coefficient=0.6;
+                float  wheel_previewDistance_min=5;
+                float  wheel_change_limit_switch=0;
+                float  wheel_max_change=10;
+
+            /*****************
+             * **** path planning *****
+             * ***************/
+                float road_width = 3.5; //道路宽度
+                int now_road_num = 0;  //车辆当前所在道路编号
+
+
+            /*****************
+             * **** frenet path planning *****
+             * ***************/
+                double MAX_SPEED = 50.0 / 3.6;     // 最大速度 [m/s]
+                double MAX_ACCEL = 10;            // 最大加速度[m/ss]
+                double MAX_CURVATURE = 10;        // 最大曲率 [1/m]
+                double MIN_ROAD_OFFSET = -3.5;     // 最小道路偏移度 [m]。可以为负数。
+                double MAX_ROAD_WIDTH = 3.5;       // 最大道路宽度 [m]。过小可能不具有避障功能。
+                double D_ROAD_W = 3.5;             // 道路宽度采样间隔 [m]
+                double DT = 0.2;                   // Delta T [s]。总的预测时间的增量。
+                double MAXT = 4.0;                 // 最大预测时间 [s]
+                double MINT = 2.0;                 // 最小预测时间 [s]
+                double D_POINT_T = 0.04;           // 时间增量 [s]。用于控制每条轨迹生成轨迹点的密度。
+                double TARGET_SPEED = 30.0 / 3.6;  // 目标速度（即纵向的速度保持） [m/s]
+                double D_T_S = 5.0 / 3.6;          // 目标速度采样间隔 [m/s]
+                double N_S_SAMPLE = 1;             // sampling number of target speed
+                double ROBOT_RADIUS = 2.0;         // robot radius [m]
+
+                // 损失函数权重
+                double KJ = 0.1;
+                double KT = 0.1;
+                double KD = 1.0;
+                double KLAT = 1.0;
+                double KLON = 1.0;
+
+
+    };
+        typedef boost::serialization::singleton<iv::decition::ParameterStatus> ParameterStatusSingleton;
+}
+#define ServiceParameterStatus iv::decition::ParameterStatusSingleton::get_mutable_instance()
+
+}
+
+
+#endif // PARAMETER_STATUS_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/polyfit.cpp

@@ -0,0 +1,425 @@
+// Name: polyfit.c
+// Description: Simple polynomial fitting functions.
+// Author: Henry M. Forson, Melbourne, Florida USA
+
+//------------------------------------------------------------------------------------
+// MIT License
+//
+// Copyright (c) 2020 Henry M. Forson
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//------------------------------------------------------------------------------------
+
+#include <math.h>       // pow()
+#include <stdbool.h>    // bool
+#include <stdio.h>      // printf()
+#include <stdlib.h>     // calloc()
+#include <string.h>     // strlen()
+#include <vector>
+
+#include "polyfit.h"
+
+// Define SHOW_MATRIX to display intermediate matrix values:
+// #define SHOW_MATRIX 1
+
+// Structure of a matrix.
+typedef struct matrix_s
+{
+	int	rows;
+	int	cols;
+	double *pContents;
+} matrix_t;
+
+// MACRO to access a value with a matrix.
+#define MATRIX_VALUE_PTR( pA, row, col )  (&(((pA)->pContents)[ (row * (pA)->cols) + col]))
+
+#ifdef SHOW_MATRIX
+#define showMatrix( x ) do {\
+    printf( "   @%d: " #x " =\n", __LINE__ ); \
+    reallyShowMatrix( x ); \
+    printf( "\n" ); \
+} while( 0 )
+#else   // SHOW_MATRIX
+#define showMatrix( x )
+#endif   // SHOW_MATRIX
+
+
+//------------------------------------------------
+// Private Function Prototypes
+//------------------------------------------------
+
+static matrix_t *   createMatrix( int rows, int cols );
+static void         destroyMatrix( matrix_t *pMat );
+#ifdef SHOW_MATRIX
+static void         reallyShowMatrix( matrix_t *pMat );
+#endif  // SHOW_MATRIX
+static matrix_t *   createTranspose( matrix_t *pMat );
+static matrix_t *   createProduct( matrix_t *pLeft, matrix_t *pRight );
+
+
+//=========================================================
+//      Global function definitions
+//=========================================================
+
+
+//--------------------------------------------------------
+// polyfit()
+// Computes polynomial coefficients that best fit a set
+// of input points.
+//
+// The degree of the fitted polynomial is one less than
+// the count of elements in the polynomial vector.
+//
+// Uses matrix algebra of the form:
+//              A * x = b
+// To solve the MLS equation:
+//              (AT) * A * x = (AT) * b
+// where (AT) is the transpose of A.
+//
+// If the n+1 input points are {(x0, y0), (x1, y1), ... (xn, yn)},
+// then the i'th row of A is: {(xi)^0, (xi)^1, ... (xn)^n},
+// and the i'th row of b is: {yi}.
+//
+// Returns   0 if success, 
+//          -1 if passed a NULL pointer,
+//          -2 if (pointCount < coefficientCount),
+//          -3 if unable to allocate memory,
+//          -4 if unable to solve equations.
+//--------------------------------------------------------
+//int polyfit( int pointCount, point_t pointArray[],  int coeffCount, double coeffArray[] )
+int polyfit( int pointCount, std::vector<double> xValues, std::vector<double> yValues, int coefficientCount, double *coefficientResults )
+
+//int polyfit( int pointCount, double *xValues, double *yValues, int coefficientCount, double *coefficientResults )
+{
+    int rVal = 0;
+    int degree = coefficientCount - 1;
+
+    // Check that the input pointers aren't null.
+//    if( (NULL == xValues) || (NULL == yValues) || (NULL == coefficientResults) )
+//    {
+//        return -1;
+//    }
+    // Check that pointCount >= coefficientCount.
+    if(pointCount < coefficientCount)
+    {
+        return -2;
+    }
+
+    // printf( "pointCount = %d:", pointCount );
+
+    // for( i = 0; i < pointCount; i++ )
+    // {
+    //     printf( " ( %f, %f )", xValues[i], yValues[i] );
+    // }
+    // printf( "\n");
+
+    // printf( "coefficientCount = %d\n", coefficientCount );
+
+    // Make the A matrix:
+    matrix_t *pMatA = createMatrix( pointCount, coefficientCount );
+    if( NULL == pMatA)
+    {
+        return -3;
+    }
+
+    for( int r = 0; r < pointCount; r++)
+    {
+        for( int c = 0; c < coefficientCount; c++)
+        {
+            *(MATRIX_VALUE_PTR(pMatA, r, c)) = pow((xValues[r]), (double) (degree -c));
+        }
+    }
+
+    showMatrix( pMatA );
+
+    // Make the b matrix
+    matrix_t *pMatB = createMatrix( pointCount, 1);
+    if( NULL == pMatB )
+    {
+        return -3;
+    }
+
+    for( int r = 0; r < pointCount; r++)
+    {
+        *(MATRIX_VALUE_PTR(pMatB, r, 0)) = yValues[r];
+    }
+
+    // Make the transpose of matrix A
+    matrix_t * pMatAT = createTranspose( pMatA );
+    if( NULL == pMatAT )
+    {
+        return -3;
+    }
+
+    showMatrix( pMatAT );
+
+    // Make the product of matrices AT and A:
+    matrix_t *pMatATA = createProduct( pMatAT, pMatA );
+    if( NULL == pMatATA )
+    {
+        return -3;
+    }
+
+     showMatrix( pMatATA );
+
+    // Make the product of matrices AT and b:
+    matrix_t *pMatATB = createProduct( pMatAT, pMatB );
+    if( NULL == pMatATB )
+    {
+        return -3;
+    }
+
+    showMatrix( pMatATB );
+
+    // Now we need to solve the system of linear equations,
+    // (AT)Ax = (AT)b for "x", the coefficients of the polynomial.
+
+    for( int c = 0; c < pMatATA->cols; c++ )
+    {
+        int pr = c;     // pr is the pivot row.
+        double prVal = *MATRIX_VALUE_PTR(pMatATA, pr, c);
+        // If it's zero, we can't solve the equations.
+        if( 0.0 == prVal )
+        {
+            // printf( "Unable to solve equations, pr = %d, c = %d.\n", pr, c );
+            showMatrix( pMatATA );
+            rVal = -4;
+            break;
+        }
+        for( int r = 0; r < pMatATA->rows; r++)
+        {
+            if( r != pr )
+            {
+                double targetRowVal = *MATRIX_VALUE_PTR(pMatATA, r, c);
+                double factor = targetRowVal / prVal;
+                for( int c2 = 0; c2 < pMatATA->cols; c2++ )
+                {
+                    *MATRIX_VALUE_PTR(pMatATA, r, c2) -=  *MATRIX_VALUE_PTR(pMatATA, pr, c2) * factor; 
+                    // printf( "c = %d, pr = %d, r = %d, c2=%d, targetRowVal = %f, prVal = %f, factor = %f.\n",
+                    //         c, pr, r, c2, targetRowVal, prVal, factor );
+
+                    // showMatrix( pMatATA );
+                   
+                }
+                *MATRIX_VALUE_PTR(pMatATB, r, 0) -=  *MATRIX_VALUE_PTR(pMatATB, pr, 0) * factor;
+
+                showMatrix( pMatATB );
+            }
+        }
+    }
+    for( int c = 0; c < pMatATA->cols; c++ )
+    {
+        int pr = c;
+        // now, pr is the pivot row.
+        double prVal = *MATRIX_VALUE_PTR(pMatATA, pr, c);
+        *MATRIX_VALUE_PTR(pMatATA, pr, c) /= prVal;
+        *MATRIX_VALUE_PTR(pMatATB, pr, 0) /= prVal;
+    }
+
+    showMatrix( pMatATA );
+
+    showMatrix( pMatATB );
+
+    for( int i = 0; i < coefficientCount; i++)
+    {
+        coefficientResults[i] = *MATRIX_VALUE_PTR(pMatATB, i, 0);
+    }
+
+    
+    destroyMatrix( pMatATB );
+    destroyMatrix( pMatATA );
+    destroyMatrix( pMatAT );
+
+    destroyMatrix( pMatA );
+    destroyMatrix( pMatB );
+    return rVal;
+}
+
+//--------------------------------------------------------
+// polyToString()
+// Produces a string representation of a polynomial from
+// its coefficients.
+// Returns 0 on success.
+//--------------------------------------------------------
+int polyToString( char *stringBuffer, size_t stringBufferSz, int coeffCount, double *coefficients )
+{
+    bool isThisTheFirstTermShown = true;
+    if( (NULL == stringBuffer) || (NULL == coefficients) )
+    {
+        return -1;  // NULL pointer passed as a parameter
+    }
+        if( (0 == stringBufferSz) || (coeffCount <= 0) )
+    {
+        return -2;  // parameter out of range.
+    }
+
+    stringBuffer[0] = 0;
+
+    for( int i = 0; i < coeffCount; i++)
+    {
+        int exponent = (coeffCount - 1) - i;
+        bool isTermPrintable = (coefficients[i] != 0.0);
+        if( isTermPrintable )
+        {
+            int stringIndex = strlen( stringBuffer );           // Index of where to write the next term.
+            char *pNext = &(stringBuffer[ stringIndex ]);       // Pointer to where to write the next term.
+            int remainingSize = stringBufferSz - stringIndex;   // Space left in buffer.
+ 
+            if( 0 == exponent )
+            {
+                snprintf( pNext, remainingSize, "%s%f", isThisTheFirstTermShown ? "" : " + ", coefficients[ i ] );
+            }
+            else if( 1 == exponent)
+            {
+                snprintf( pNext, remainingSize, "%s(%f * x)", isThisTheFirstTermShown ? "" : " + ", coefficients[ i ] );
+            }
+            else
+            {
+                snprintf( pNext, remainingSize, "%s(%f * x^%d)", isThisTheFirstTermShown ? "" : " + ", coefficients[i], exponent );
+            }
+            isThisTheFirstTermShown = false;
+        }
+    }
+    return 0;
+}
+
+//=========================================================
+//      Private function definitions
+//=========================================================
+
+#ifdef SHOW_MATRIX
+//--------------------------------------------------------
+// reallyShowMatrix()
+// Printf the contents of a matrix
+//--------------------------------------------------------
+static void reallyShowMatrix( matrix_t *pMat )
+{
+    for( int r = 0; r < pMat->rows; r++ )
+    {
+        for( int c = 0; c < pMat->cols; c++)
+        {
+            printf( "   %f", *MATRIX_VALUE_PTR(pMat, r, c));
+        }
+        printf( "\n" );
+    }
+}
+#endif  // SHOW_MATRIX
+
+//--------------------------------------------------------
+// createTranspose()
+// Returns the transpose of a matrix, or NULL.
+//
+// The caller must free both the allocated matrix
+// and its contents array.
+//--------------------------------------------------------
+static matrix_t * createTranspose( matrix_t *pMat )
+{
+    matrix_t *rVal = (matrix_t *) calloc(1, sizeof(matrix_t));
+    rVal->pContents = (double *) calloc( pMat->rows * pMat->cols, sizeof( double ));
+    rVal->cols = pMat->rows;
+    rVal->rows = pMat->cols;
+    for( int r = 0; r < rVal->rows; r++ )
+    {
+        for( int c = 0; c < rVal->cols; c++ )
+        {
+            *MATRIX_VALUE_PTR(rVal, r, c) = *MATRIX_VALUE_PTR(pMat, c, r);
+        }
+    }
+    return rVal;
+}
+
+//--------------------------------------------------------
+// createProduct()
+// Returns the product of two matrices, or NULL.
+//
+// The caller must free both the allocated product matrix
+// and its contents array.
+//--------------------------------------------------------
+static matrix_t * createProduct( matrix_t *pLeft, matrix_t *pRight )
+{
+    matrix_t *rVal = NULL;
+    if( (NULL == pLeft) || (NULL == pRight) || (pLeft->cols != pRight->rows) )
+    {
+        printf( "Illegal parameter passed to createProduct().\n");
+    }
+    else
+    {
+        // Allocate the product matrix.
+        rVal = (matrix_t *) calloc(1, sizeof(matrix_t));
+        rVal->rows = pLeft->rows;
+        rVal->cols = pRight->cols;
+        rVal->pContents = (double *) calloc( rVal->rows * rVal->cols, sizeof( double ));
+
+        // Initialize the product matrix contents:
+        // product[i,j] = sum{k = 0 .. (pLeft->cols - 1)} (pLeft[i,k] * pRight[ k, j])
+        for( int i = 0; i < rVal->rows; i++)
+        {
+            for( int j = 0; j < rVal->cols; j++ )
+            {
+                for( int k = 0; k < pLeft->cols; k++)
+                {
+                    *MATRIX_VALUE_PTR(rVal, i, j) +=
+                        (*MATRIX_VALUE_PTR(pLeft, i, k)) * (*MATRIX_VALUE_PTR(pRight, k, j));
+                }
+            }
+        }
+    }    
+       
+    return rVal;
+}
+
+//--------------------------------------------------------
+// destroyMatrix()
+// Frees both the allocated matrix and its contents array.
+//--------------------------------------------------------
+static void destroyMatrix( matrix_t *pMat )
+{
+    if(NULL != pMat)
+    {
+        if(NULL != pMat->pContents)
+        {
+            free(pMat->pContents);
+        }
+        free( pMat );
+    }
+}
+
+//--------------------------------------------------------
+// createMatrix()
+// Allocates the matrix and clears its contents array.
+//--------------------------------------------------------
+static matrix_t *createMatrix( int rows, int cols )
+{
+    matrix_t *rVal = (matrix_t *) calloc(1, sizeof(matrix_t));
+    if(NULL != rVal)
+    {
+        rVal->rows = rows;
+        rVal->cols = cols;
+        rVal->pContents = (double *) calloc( rows * cols, sizeof( double ));
+        if(NULL == rVal->pContents)
+        {
+            free( rVal );
+            rVal = NULL;
+        }
+    }
+
+    return rVal;
+}
+
+ 

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/polyfit.h

@@ -0,0 +1,57 @@
+// file: polyfit.h
+// Description:	Header file for MLS polynomial fitting.
+// Author: Henry Forson, Melbourne, FL
+
+//------------------------------------------------------------------------------------
+// MIT License
+//
+// Copyright (c) 2020 Henry M. Forson
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//------------------------------------------------------------------------------------
+
+#ifndef POLYFIT_H
+#define POLYFIT_H
+
+#include <vector>
+//------------------------------------------------
+// Function Prototypes
+//------------------------------------------------
+
+//--------------------------------------------------------
+// polyfit()
+// Computes polynomial coefficients that best fit a set
+// of input points.
+//
+// Returns 0 if success.
+//--------------------------------------------------------
+//int polyfit( int pointCount, double *xValues, double *yValues, int coefficientCount, double *coefficientResults );
+int polyfit( int pointCount, std::vector<double> xValues, std::vector<double> yValues, int coefficientCount, double *coefficientResults );
+
+//--------------------------------------------------------
+// polyToString()
+// Produces a string representation of a polynomial from
+// its coefficients.
+// Returns 0 on success.
+//--------------------------------------------------------
+int polyToString( char *stringBuffer, size_t stringBufferSz, int coeffCount, double *coefficients );
+
+
+
+#endif	// POLYFIT_H

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/transfer.cpp

@@ -0,0 +1,416 @@
+#include <decition/adc_tools/transfer.h>
+#include <decition/decition_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include "PolynomialRegression.h"
+#include <common/car_status.h>
+using namespace std;
+#define PI (3.1415926535897932384626433832795)
+
+
+
+///大地转车体
+//iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
+//{
+//    double x_vehicle, y_vehicle;
+//    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
+
+//    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
+//    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
+
+//    x_vehicle = -cos(angle) * distance;
+//    y_vehicle = sin(angle) * distance;
+//    return  iv::Point2D(x_vehicle, y_vehicle);
+//}
+
+iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    double x_vehicle, y_vehicle;
+
+    double x_t= x_path- pos.gps_x;
+    double y_t= y_path- pos.gps_y;
+
+    x_vehicle = x_t * cos(pos.ins_heading_angle * PI / 180) - y_t * sin(pos.ins_heading_angle * PI / 180);
+    y_vehicle = x_t * sin(pos.ins_heading_angle * PI / 180) + y_t * cos(pos.ins_heading_angle * PI / 180);
+    return  iv::Point2D(x_vehicle, y_vehicle);
+}
+///车体转大地
+iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+//    iv::GPS_INS gps_ins;
+//    double x_vehicle, y_vehicle;
+//    double theta = atan2(x_path, y_path);
+
+//    double distance = sqrt(x_path * x_path + y_path * y_path);
+//    double angle = (pos.ins_heading_angle / 180 * PI + theta);
+
+//    x_vehicle = pos.gps_x + distance * sin(angle);
+//    y_vehicle = pos.gps_y + distance * cos(angle);
+//    gps_ins.gps_x = x_vehicle;
+//    gps_ins.gps_y = y_vehicle;
+
+
+    iv::GPS_INS gps_ins;
+    double x_vehicle, y_vehicle;
+    double x_t= x_path;
+    double y_t= y_path;
+
+    x_vehicle = x_t * cos(pos.ins_heading_angle * PI / 180) + y_t * sin(pos.ins_heading_angle * PI / 180)+pos.gps_x;
+    y_vehicle = -x_t * sin(pos.ins_heading_angle * PI / 180) + y_t * cos(pos.ins_heading_angle * PI / 180)+pos.gps_y;
+    gps_ins.gps_x = x_vehicle;
+    gps_ins.gps_y = y_vehicle;
+
+    return  gps_ins;
+}
+
+
+// 1D [ x , y ] → [ s , d ]
+//步骤：先找到距离被测点最近的S的坐标，最临近点的逻辑查看decition模块；
+//以S对应的（x,y）求被测点的S和D 坐标
+//rs是最临近被侧点的地图参考轨点的S值
+//rx,ry分别是最临近的地图参考轨迹的x和Y值
+//rtheta是最临近参考轨迹点的航向角
+//x，y是被转换的笛卡尔坐标的点
+
+iv::Point2D iv::decition::cartesian_to_frenet1D(std::vector<GPSData> gpsMap,double x, double y)
+{
+    iv::Point2D point;
+    double s_condition = 0.0;
+    double d_condition = 0.0;
+
+
+    float minDis = 100;
+    int index=-1;
+
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+    iv::GPS_INS p1;
+    p1.gps_x=x;
+    p1.gps_y=y;
+
+    ///寻找最近点
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(p1, (*gpsMap[i]));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis = tmpdis;
+        }
+    }
+
+    ///计算全局地图S
+    if(fabs((*gpsMap[gpsMap.size()-1]).frenet_s)<1.0e-6){
+        double sum_s=0.0;
+        for(int i=0;i<gpsMap.size();i++)
+        {
+            if (i==0)
+            {
+                sum_s=0.0;
+                (*gpsMap[i]).frenet_s=0;
+            }
+            else
+            {
+
+                sum_s = sum_s+sqrt(pow(((*gpsMap[i]).gps_x-(*gpsMap[i-1]).gps_x),2) + pow(((*gpsMap[i]).gps_y-(*gpsMap[i-1]).gps_y),2));
+               (*gpsMap[i]).frenet_s=sum_s;
+            }
+        }
+    }
+
+    double rx=(*gpsMap[index]).gps_x;
+    double ry=(*gpsMap[index]).gps_y;
+    double rs=(*gpsMap[index]).frenet_s;
+
+    double rtheta=PI/2-((*gpsMap[index]).ins_heading_angle* PI / 180);
+
+    double dx = x - rx;
+    double dy = y - ry;
+
+    double  cos_theta_r = cos(rtheta);
+    double sin_theta_r = sin(rtheta);
+
+    double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
+    d_condition = copysign(sqrt(dx * dx + dy * dy), cross_rd_nd);
+
+    s_condition = rs;
+    point.s = s_condition;
+    point.d = d_condition;
+    point.x =x;
+    point.y =y;
+
+    return point;
+}
+
+// 1D [ s , d ] → [ x , y ]
+//rs是距离被测点最近的S坐标
+//rx和ry是最近点s对应的X和Y 坐标
+//rtheta是最近点的航向角
+iv::Point2D iv::decition::frenet_to_cartesian1D(std::vector<GPSData> gpsMap,double s_condition, double d_condition,int index)
+ {
+    iv::Point2D point;
+    vector<double> X;
+    vector<double> Y;
+    std::vector<double> polyd_coeffs_x;//以s为自变量，x为因变量的多项式系数
+    std::vector<double> polyd_coeffs_y;//以s为参数的Y多项式系数y=polyd_coeffs_y[0]+polyd_coeffs_y[1]*x+polyd_coeffs_y[2]*x*x
+    double sum_s=0.0;
+
+    ///计算全局地图S
+    if(fabs((*gpsMap[gpsMap.size()-1]).frenet_s)<1.0e-6){
+
+        for(unsigned int i=0;i<gpsMap.size();i++)
+        {
+            if (i==0)
+            {
+                sum_s=0.0;
+                (*gpsMap[i]).frenet_s=0;
+            }
+            else
+            {
+
+                sum_s = sum_s+sqrt(pow(((*gpsMap[i]).gps_x-(*gpsMap[i-1]).gps_x),2) + pow(((*gpsMap[i]).gps_y-(*gpsMap[i-1]).gps_y),2));
+               (*gpsMap[i]).frenet_s=sum_s;
+            }
+        }
+    }else{
+        sum_s=(*gpsMap[gpsMap.size()-1]).frenet_s;
+    }
+//    float minDis = 5;
+//    int near_index=-1;
+//int map_size=gpsMap.size();
+//int s_index=(int)s_condition*10+index;
+//    int startIndex = max(0,s_index-500);     // startIndex = 0 则每一次都是遍历整条地图路线
+//    int endIndex = min(s_index+500,map_size);
+
+    ///寻找最近点
+//    for (int j = startIndex; j < endIndex; j++)
+//    {
+//        int i = (j + gpsMap.size()) % gpsMap.size();
+//        double tmpdis = fabs((*gpsMap[i]).frenet_s-s_condition);
+
+//        if (tmpdis < minDis)
+//        {
+//            near_index = i;
+//            minDis = tmpdis;
+//        }
+//    }
+//    if(near_index!=-1){
+//        std::cout<<"The reference point s and s_condition[0] don't match**********index="<<index<<"index="<<near_index<<std::endl;
+//        index=near_index;
+//    }
+
+    //[ s , d ] → [ x , y ]测试
+    //根据给定的S，根据向前向后的查找距离把参考线取段，look_forward 和look_backward,
+    //一种方法是计算s向前向后看得范围（现在使用的这种方法）
+    //一种方法是找到距离S最近的地图上的点好，向前向后查看范围
+    double sfov_min = s_condition - 40;//向前向后的查看距离是15米
+    double sfov_max = s_condition + 40;
+    double sfov_shit = 0.0;
+
+    if (sfov_min < 0)
+    {
+        sfov_shit = 0-sfov_min;
+    }
+    else if (sfov_max > sum_s)
+    {
+        sfov_shit = sum_s - sfov_max;
+    }
+    sfov_min += sfov_shit;
+    sfov_max += sfov_shit;
+
+    std::vector<double> x_fov;
+    std::vector<double> y_fov;
+    std::vector<double> s_fov;//分别存储所取参考轨迹点x,y和S
+    int fov_num=0;
+    int s_index=(int)(s_condition-(*gpsMap[index]).frenet_s)*10+index;
+    int start_index=max(0,s_index-500);
+    int map_size=gpsMap.size();
+    int end_index=min(s_index+500,map_size-1);
+
+    int map_max=(*gpsMap[map_size-1]).frenet_s;
+    double s_index_min=(*gpsMap[start_index]).frenet_s;
+    double s_index_max=(*gpsMap[end_index]).frenet_s;
+    //std::cout<<"s_min="<<sfov_min<<"s_max="<<sfov_max<<"s_index_min="<<s_index_min<<"s_index_max="<<s_index_max<<"map_max"<<map_max<<std::endl;
+
+    for (int i=start_index; i<end_index;i=i+6)
+    {
+        if (((*gpsMap[i]).frenet_s>=sfov_min) && ((*gpsMap[i]).frenet_s<= sfov_max))
+        {
+            x_fov.push_back((*gpsMap[i]).gps_x);
+            y_fov.push_back((*gpsMap[i]).gps_y);
+            s_fov.push_back((*gpsMap[i]).frenet_s);
+            fov_num++;
+        }
+     }
+
+     ///分别求出X和Y 的五次多项式,x=f(s),y=f(s)
+     for (size_t i = 0; i < s_fov.size(); i++)
+     {
+        X.push_back(x_fov[i]);
+        Y.push_back(s_fov[i]) ;
+     }
+     int orders = 5;
+     std::vector<double> coeffx = PolynomialRegression::fit(Y, X, orders);
+     polyd_coeffs_x.clear();
+     for(int i=0;i<orders+1;i++)
+     {
+       polyd_coeffs_x.push_back(coeffx[i]);
+     }
+
+     X.clear();
+     Y.clear();
+     for (size_t i = 0; i < s_fov.size(); i++)
+     {
+       X.push_back(y_fov[i]);
+       Y.push_back(s_fov[i]) ;
+     }
+     std::vector<double> coeffy = PolynomialRegression::fit(Y, X, orders);
+     polyd_coeffs_y.clear();
+
+     for(int i=0;i<orders+1;i++)
+     {
+       polyd_coeffs_y.push_back(coeffy[i]);
+     }
+     ///求出[ s , d ]中相应s在曲线上的坐标值及一阶导数
+     double rs = s_condition;
+     double rx = poly1d(polyd_coeffs_x,rs);
+     double ry = poly1d(polyd_coeffs_y,rs);
+     double drx = deriv(polyd_coeffs_x,rs);
+     double dry = deriv(polyd_coeffs_y,rs);
+     double rtheta = atan2(dry, drx);
+
+    /// 1D [ s , d ] → [ x , y ]
+//    if (fabs(rs - s_condition) >= 1.0e-6)
+//       std::cout<<"The reference point s and s_condition[0] don't match"<<std::endl;
+
+    double cos_theta_r = cos(rtheta);
+    double sin_theta_r = sin(rtheta);
+
+    double x = rx - sin_theta_r * d_condition;
+    double y = ry + cos_theta_r * d_condition;
+    point.x =x;
+    point.y =y;
+    point.s =s_condition;
+    point.d =d_condition;
+    return point;
+ }
+double iv::decition::poly1d(const std::vector<double> &array,double s) //由S位置计算出当前S对应的X和Y坐标
+{
+    double ans=0.0;
+    for(unsigned int i=0;i<array.size();i++)
+    {
+        ans= ans+array[i]*pow(s,i);
+    }
+//    ans= array[0]+array[1]*s+;
+   return ans;
+}
+
+double iv::decition::deriv(const std::vector<double> &array,double s)//多项式的一阶导数
+{
+    double ans=0.0;
+    for(unsigned int i=1;i<array.size();i++)
+    {
+        ans= ans+i*array[i]*pow(s,i-1);
+    }
+   return ans;
+}
+
+double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
+{
+    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
+}
+
+double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
+{
+    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
+}
+
+
+
+
+//GPS转大地坐标
+void iv::decition::BLH2XYZ(iv::GPS_INS gp)
+{
+    int nFlag = 2;
+
+    double B = gp.gps_lat;
+
+    double L = gp.gps_lng;
+
+
+
+    double N, E, h;
+    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
+
+    double a = 6378245.0;            //地球半径  北京6378245
+    double F = 298.257223563;        //地球扁率
+    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
+
+    double f = 1 / F;
+    double b = a * (1 - f);
+    double ee = (a * a - b * b) / (a * a);
+    double e2 = (a * a - b * b) / (b * b);
+    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
+    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
+    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
+    double gm = 15 * n2 / 16 - 15 * n4 / 32;
+    double dt = -35 * n3 / 48 + 105 * n5 / 256;
+    double ep = 315 * n4 / 512;
+
+    B = B * iPI;
+    L = L * iPI;
+    L0 = L0 * iPI;
+
+    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
+    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
+    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
+    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
+    double yt = e2 * cos(B) * cos(B);
+    N = lB;
+    N += t * Nn * cl2 / 2;
+    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
+    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
+    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
+
+    E = Nn * cl;
+    E += Nn * cl3 * (1 - t2 + yt) / 6;
+    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
+    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
+
+    E += 500000;
+    if (nFlag == 1)
+    {
+        //UTM投影
+        N = 0.9996 * N;
+        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
+    }
+    if (nFlag == 2)
+    {
+        //UTM投影
+        N = 0.9999 * N;
+        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
+    }
+
+    //原
+    //pt2d.x = N;
+    //pt2d.y = E;
+    //
+    gp.gps_x = E - 280000;
+    gp.gps_y = N - 4325000;
+
+}
+
+
+double iv::decition::GetL0InDegree(double dLIn)
+{
+    //3°带求带号及中央子午线经度(d的形式)
+    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
+    double L = dLIn;//d.d
+    double L_ddd_Style = L;
+    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
+    double L0 = ZoneNumber * 3.0;//degree
+    return L0;
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/adc_tools/transfer.h

@@ -0,0 +1,33 @@
+#pragma once
+#ifndef _IV_DECITION_TRANSFER_
+#define _IV_DECITION_TRANSFER_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+
+namespace iv {
+    namespace decition {
+        //根据传感器传输来的信息作出决策
+
+        double GetL0InDegree(double dLIn);
+
+        void BLH2XYZ(GPS_INS gps_ins);
+
+        ///大地转车体
+        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
+
+        ///车体转大地
+        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
+        double GetDistance(Point2D x1, Point2D x2);
+        double GetDistance(GPS_INS p1, GPS_INS p2);
+
+        ///大地转frenet
+        Point2D cartesian_to_frenet1D(std::vector<GPSData> gpsMap,double x, double y);
+        Point2D frenet_to_cartesian1D(std::vector<GPSData> gpsMap,double s_condition, double d_condition,int index);
+        double poly1d(const std::vector<double> &array,double s);
+        double deriv(const std::vector<double> &array,double s);
+    }
+}
+
+#endif // ! _IV_DECITION_TRANSFER_

src/decition/decition_brain_sf_add_frenet_plan/decition/brain.cpp

@@ -0,0 +1,1647 @@
+#include <decition/brain.h>
+#include <fstream>
+#include <time.h>
+#include <exception>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <common/logout.h>
+#include <decition/adc_tools/transfer.h>
+
+#include <iostream>
+#include "xmlparam.h"
+#include "qcoreapplication.h"
+#include <fstream>
+#include <QTime>
+
+#include "ivfault.h"
+extern iv::Ivfault * givfault;
+
+using namespace std;
+
+extern std::string gstrmemdecition;
+extern std::string gstrmembrainstate;
+extern iv::Ivlog * givlog;
+extern std::string gstrmemchassis;
+extern std::string gstrmembraincarstate;
+
+#define LOG_ENABLE
+
+namespace iv {
+namespace decition {
+iv::decition::BrainDecition * gbrain;
+
+
+        void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateMap(strdata,nSize);
+            gbrain->UpdateSate();
+        }
+
+        void ListenVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateVbox(strdata,nSize);
+            gbrain->UpdateSate();
+        }
+
+        void ListenHMI(const char * strdata,const unsigned int nSize ,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateHMI(strdata,nSize);
+        }
+
+        void ListenPlatform(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdatePlatform(strdata,nSize);
+        }
+
+        void ListenGroup(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            gbrain->UpdateGroupInfo(strdata,nSize);
+        }
+
+        void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+        {
+           gbrain->GetFusion(strdata,nSize);
+        }
+
+    /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        {
+            iv::formation_map_index::map map;
+            if(false == map.ParseFromArray(strdata,nSize))
+            {
+                givlog->error("GPS","[%s] Listencansend Parse fail.",__func__);
+                return;
+            }else{
+               gbrain->UpdateSate();
+            }
+        //    map.index()-1  map index num.
+        }*/
+
+    }
+
+}
+
+
+
+iv::decition::BrainDecition::BrainDecition()
+{
+    givfault->SetFaultState(0,0,"Application Initializing.");
+    //    mpasd = new Adcivstatedb();
+    look1 = 0.0;
+    look2 = 0.0;
+    look3 = 0.0;
+    look4 = 0.0;
+    look5 = 0.0;
+    look6 = 0.0;
+    look7 = 0.0;
+    obs_lidar_grid_ = NULL;
+    old_obs_lidar_grid_ = NULL;
+    obs_camera_ = NULL;
+    obs_fusion_grid_ = NULL;
+    //    controller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());
+
+    //    carcall = new iv::carcalling::carcalling();
+    eyes = new iv::perception::Eyes();
+    //	decitionMaker = new iv::decition::DecitionMaker();
+
+    decitionGps00 = new iv::decition::DecideGps00();
+    decitionLine00=new iv::decition::DecideLine00();
+
+    //	decitionGpsUnit = new iv::decition::DecideGpsUnit();
+
+    //	decitionExecuter = new iv::decition::DecitionExecuter(controller);
+
+    iv::decition::gbrain = this;
+
+
+    mpa = iv::modulecomm::RegisterRecv("tracemap",iv::decition::ListenTraceMap);
+
+    mpvbox = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);
+
+    mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
+    mpaCarstate= iv::modulecomm::RegisterSend(gstrmembraincarstate.data(),10000,10);
+
+    mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
+
+
+    mpaToPlatform = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
+    mpaDecition = iv::modulecomm::RegisterSend(gstrmemdecition.data(),10*sizeof(iv::decition::DecitionBasic),1);
+
+    mpaHMI = iv::modulecomm::RegisterRecv("hmi",iv::decition::ListenHMI);
+
+    void * ppad = iv::modulecomm::RegisterRecv("pad",iv::decition::ListenHMI);
+
+    mpaPlatform = iv::modulecomm::RegisterRecv("platformfrom",iv::decition::ListenPlatform);
+
+    mpaGroup = iv::modulecomm::RegisterRecv("p900_send",iv::decition::ListenGroup);
+ //   iv::modulecomm::RegisterRecv("map_index",ListenMap_change_req);
+
+
+
+
+    mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
+
+    ModuleFun funchassis =std::bind(&iv::decition::BrainDecition::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpaChassis = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);
+
+
+    ModuleFun fungroupgrpc =std::bind(&iv::decition::BrainDecition::UpdateGRPCGroupMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mapgrpcgroup = iv::modulecomm::RegisterRecvPlus("groupmsg",fungroupgrpc);
+
+    mpaPlanTrace = iv::modulecomm::RegisterSend("plantrace",100000,1);
+    mpaObsTraceLeft = iv::modulecomm::RegisterSend("obstraceleft",100000,1);
+    mpaObsTraceRight = iv::modulecomm::RegisterSend("obstraceright",100000,1);
+
+
+    mTime.start();
+    mnOldTime = mTime.elapsed();
+
+}
+
+iv::decition::BrainDecition::~BrainDecition()
+{
+
+    iv::modulecomm::Unregister(mpaObsTraceRight);
+    iv::modulecomm::Unregister(mpaObsTraceLeft);
+    iv::modulecomm::Unregister(mpaPlanTrace);
+    iv::modulecomm::Unregister(mpaChassis);
+    iv::modulecomm::Unregister(mpamapreq);
+    iv::modulecomm::Unregister(mpaGroup);
+    iv::modulecomm::Unregister(mpaPlatform);
+    iv::modulecomm::Unregister(mpaHMI);
+    iv::modulecomm::Unregister(mpaDecition);
+    iv::modulecomm::Unregister(mpaToPlatform);
+    iv::modulecomm::Unregister(mpfusion);
+    iv::modulecomm::Unregister(mpaCarstate);
+    iv::modulecomm::Unregister(mpaVechicleState);
+    iv::modulecomm::Unregister(mpvbox);
+    iv::modulecomm::Unregister(mpa);
+
+    delete eyes;
+    std::cout<<"Brain Unialize."<<std::endl;
+}
+
+
+void iv::decition::BrainDecition::inialize() {
+    //	controller->inialize();
+    eyes->inialize();
+    //    carcall->start();
+}
+
+
+void iv::decition::BrainDecition::run() {
+
+    givfault->SetFaultState(0,0,"Start Running.");
+    double last = 0;
+
+    iv::decition::Decition decition_gps = NULL;
+    iv::decition::Decition decition_lidar = NULL;
+    iv::decition::Decition decition_radar = NULL;
+    iv::decition::Decition decition_camera = NULL;
+    iv::decition::Decition decition_localmap = NULL;
+
+    iv::decition::Decition dgtemp(new iv::decition::DecitionBasic);
+    decition_gps = dgtemp;
+    decition_gps->brake = 0;
+    decition_gps->accelerator = 0;
+    decition_gps->wheel_angle = 0;
+
+    //controller->auto_drive_mode_enable(true);
+
+    /*Decition test(new DecitionBasic);
+    while (true)
+    {
+        test->accelerator = 1.2;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = -1.2;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = 45;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = 0;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+        test->accelerator = 0;
+        test->wheel_angle = -60;
+        decitionExecuter->executeDecition(test);
+        Sleep(5000);
+
+    }*/
+
+    //   std::cout<<"start run."<<std::endl;
+
+    //   ServiceCarStatus.mbRunPause = false;
+
+    bool bRun;
+    int nValueSize;
+
+    //    if(mpasd->LoadState("runstate",(char *)&bRun,sizeof(bool),&nValueSize) == true)
+    //    {
+    //        if(bRun)
+    //        {
+    //            ServiceCarStatus.mbRunPause = false;
+    //        }
+    //        else
+    //        {
+    //            ServiceCarStatus.mbRunPause = true;
+    //        }
+    //    }
+
+    char * strmap = new char[10000000];
+    int nMapSize;
+
+    //    if(mpasd->LoadState("map",strmap,10000000,&nMapSize) == true)
+    //    {
+    //        UpdateMap(strmap,nMapSize);
+    //    }
+
+    QString strpath = QCoreApplication::applicationDirPath();
+    strpath = strpath + "/ADS_decision.xml";
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+
+    ServiceCarStatus.msysparam.mstrvin = xp.GetParam("vin","10000000000000001");
+    ServiceCarStatus.msysparam.mstrid = xp.GetParam("id","1");
+    ServiceCarStatus.msysparam.mstriccid = xp.GetParam("iccid","11111111111111111111");
+    ServiceCarStatus.msysparam.mvehtype= xp.GetParam("vehType","ge3");
+
+    ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
+    ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
+
+    ServiceCarStatus.msysparam.mbSideDrive = xp.GetParam("SideDrive",false);
+    ServiceCarStatus.msysparam.mfSideDis = xp.GetParam("SideDis",0.3);
+
+
+
+
+    /*=============     20200113 apollo_fu  add ===========*/
+    std::string str_zhuchetime = xp.GetParam("zhuchetime","16");
+    ServiceCarStatus.msysparam.mzhuchetime =atoi(str_zhuchetime.data());
+    /*============= end ================================== */
+
+    std::string strepsoff = xp.GetParam("epsoff","0.0");
+    ServiceCarStatus.mfEPSOff = atof(strepsoff.data());
+
+    std::string strroadmode_vel = xp.GetParam("roadmode0","10");
+    ServiceCarStatus.mroadmode_vel.mfmode0 = atof(strroadmode_vel.data());
+
+     strroadmode_vel= xp.GetParam("roadmode1","10");
+    ServiceCarStatus.mroadmode_vel.mfmode1 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode11","30");
+    ServiceCarStatus.mroadmode_vel.mfmode11 = atof(strroadmode_vel.data());
+
+
+    strroadmode_vel = xp.GetParam("roadmode14","15");
+    ServiceCarStatus.mroadmode_vel.mfmode14 = atof(strroadmode_vel.data());
+
+    //float a = ServiceCarStatus.mroadmode_vel.mfmode14;
+    //cout<<"........"<<a<<endl;
+
+    strroadmode_vel = xp.GetParam("roadmode15","15");
+    ServiceCarStatus.mroadmode_vel.mfmode15 = atof(strroadmode_vel.data());
+
+    /*==================== 20200113    apollo_fu add =================*/
+    strroadmode_vel = xp.GetParam("roadmode5","10");
+    ServiceCarStatus.mroadmode_vel.mfmode5 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode13","5");
+    ServiceCarStatus.mroadmode_vel.mfmode13 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode16","8");
+    ServiceCarStatus.mroadmode_vel.mfmode16 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode17","8");
+    ServiceCarStatus.mroadmode_vel.mfmode17 = atof(strroadmode_vel.data());
+
+    strroadmode_vel = xp.GetParam("roadmode18","5");
+    ServiceCarStatus.mroadmode_vel.mfmode18 = atof(strroadmode_vel.data());
+
+
+
+
+    /*=========================    end   =============================*/
+
+    std::string group_cotrol = xp.GetParam("group","false");
+    if(group_cotrol=="true"){
+        ServiceCarStatus.group_control=true;
+    }else{
+        ServiceCarStatus.group_control=false;
+    }
+
+    std::string speed_control = xp.GetParam("speed","false");
+    if(speed_control=="true"){
+        ServiceCarStatus.speed_control=true;
+    }else{
+        ServiceCarStatus.speed_control=false;
+    }
+
+    std::string log_on= xp.GetParam("log","false");
+    if(log_on=="true"){
+        ServiceCarStatus.txt_log_on=true;
+    }else{
+        ServiceCarStatus.txt_log_on=false;
+    }
+
+    if(ServiceCarStatus.txt_log_on==true){
+        QDateTime dt = QDateTime::currentDateTime();
+        //将数据写入文件开始
+        ofstream outfile;
+        outfile.open("control_log.txt", ostream::ate);
+        outfile.flush();
+        outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
+              <<"vehState"<<'\t'<<"PathPoint"<<'\t'<<setprecision(4)<<"dSpeed"<<'\t'<<"obsDistance"<<'\t'<<"obsSpeed"<<'\t'<<"realspeed"<<'\t'<<"minDecelerate"<<'\t'
+             <<"torque"<<'\t'<<"brake"<<'\t'<<"wheel_angle"<<'\t'<<"now_x"<<'\t'<<"now_y"<<'\t'<<"now_s"<<'\t'<<"now_d"<<endl;
+        outfile.close();
+        //将数据写入文件结束
+    }
+
+    std::string table_look_up_on= xp.GetParam("table","false");
+    if(table_look_up_on=="true"){
+        ServiceCarStatus.table_look_up_on=true;
+    }else{
+        ServiceCarStatus.table_look_up_on=false;
+    }
+
+    std::string strparklat = xp.GetParam("parklat","39.0624557");
+    std::string strparklng = xp.GetParam("parklng","117.3575493");
+    std::string strparkheading = xp.GetParam("parkheading","112.5");
+    std::string strparktype = xp.GetParam("parktype","1");
+
+    ServiceCarStatus.mfParkLat = atof(strparklat.data());
+    ServiceCarStatus.mfParkLng = atof(strparklng.data());
+    ServiceCarStatus.mfParkHeading = atof(strparkheading.data());
+    ServiceCarStatus.mnParkType = atoi(strparktype.data());
+
+    ServiceCarStatus.mLidarRotation = atof(xp.GetParam("LidarRotation","90.0").data());
+    ServiceCarStatus.mLidarRangeUnit = atof(xp.GetParam("LidarRangeUnit","5.0").data());
+
+
+
+    std::string lightlon = xp.GetParam("lightlon","0");
+    std::string lightlat = xp.GetParam("lightlat","0");
+    ServiceCarStatus.iTrafficeLightLat=atof(lightlat.data());
+    ServiceCarStatus.iTrafficeLightLon=atof(lightlon.data());
+
+
+
+    //mobileEye
+    std::string   timeWidth =xp.GetParam("waitGpsTimeWidth","5000");
+    std::string   garageLong =xp.GetParam("outGarageLong","10");
+    ServiceCarStatus.waitGpsTimeWidth=atof(timeWidth.data());
+    ServiceCarStatus.outGarageLong=atof(garageLong.data());
+    //mobileEye end
+
+
+    //lidar start
+    std::string lidarGpsXiuzheng = xp.GetParam("lidarGpsXiuzheng","1.0");
+    std::string radarGpsXiuzheng = xp.GetParam("radarGpsXiuzheng","3.0");
+    std::string rearRadarGpsXiuzheng = xp.GetParam("rearRadarGpsXiuzheng","0.5");
+    std::string rearLidarGpsXiuzheng = xp.GetParam("rearLidarGpsXiuzheng","1.0");
+    std::string frontGpsXiuzheng = xp.GetParam("frontGpsXiuzheng","3.0");
+    std::string rearGpsXiuzheng = xp.GetParam("rearGpsXiuzheng","1.0");
+    std::string gpsOffset_x = xp.GetParam("gpsOffset_X","0");
+    std::string gpsOffset_y = xp.GetParam("gpsOffset_Y","0");
+    std::string strexternmpc = xp.GetParam("ExternMPC","false");  //If Use MPC set true
+    std::string lidar_per_predict = xp.GetParam("lidarPP","false");  //If Use MPC set true
+
+    std::string groupID = xp.GetParam("groupid","1");
+    ServiceCarStatus.curID = atoi(groupID.data());
+
+     adjuseGpsLidarPosition();
+
+    if(strexternmpc == "true")
+    {
+        mbUseExternMPC = true;
+    }
+
+    if(lidar_per_predict == "true"){
+        ServiceCarStatus.useLidarPerPredict = true;
+    }
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng = atof(lidarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.radarGpsXiuzheng = atof(radarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng = atof(rearRadarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng = atof(rearLidarGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.frontGpsXiuzheng = atof(frontGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.rearGpsXiuzheng = atof(rearGpsXiuzheng.data());
+    ServiceCarStatus.msysparam.gpsOffset_x = atof(gpsOffset_x.data());
+    ServiceCarStatus.msysparam.gpsOffset_y = atof(gpsOffset_y.data());
+    // lidar end
+
+    std::string strObsPredict = xp.GetParam("obsPredict","false");  //If Use MPC set true
+    if(strObsPredict == "true")
+    {
+        ServiceCarStatus.useObsPredict = true;
+    }
+    //map
+
+    std::string inRoadAvoid = xp.GetParam("inRoadAvoid","false");  //If Use MPC set true
+    if(inRoadAvoid == "true")
+    {
+        ServiceCarStatus.inRoadAvoid = true;
+    }else{
+        ServiceCarStatus.inRoadAvoid = false;
+    }
+
+    std::string avoidObs = xp.GetParam("avoidObs","false");  //If Use MPC set true
+    if(avoidObs == "true")
+    {
+        ServiceCarStatus.avoidObs = true;
+    }else{
+        ServiceCarStatus.avoidObs = false;
+    }
+
+    mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
+
+
+    givfault->SetFaultState(0,0,"OK.");
+
+    while (eyes->isAllSensorRunning() &&(!boost::this_thread::interruption_requested()))
+    {
+        if(navigation_data.size() <1)
+        {
+            iv::map::mapreq x;
+            x.set_maptype(1);
+            int nsize = x.ByteSize();
+            char * str = new char[nsize];
+            if(x.SerializeToArray(str,nsize))
+            {
+                iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize);
+            }
+            else
+            {
+                std::cout<<"iv::map::mapreq serialize error."<<std::endl;
+            }
+            delete str;
+        }
+
+
+        iv::GPSData gps_data_;			//gps 惯导数据
+
+        //       qDebug("size = %d",navigation_data.size());
+
+
+
+        std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);
+
+        bool brun =ServiceCarStatus.mbRunPause;
+        if(ServiceCarStatus.mnBocheComplete > 0)
+        {
+            ServiceCarStatus.mbBrainCtring = false;
+            ServiceCarStatus.mbRunPause = true;
+            ServiceCarStatus.mnBocheComplete--;
+        }
+        if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
+        {
+            ServiceCarStatus.mbBrainCtring = false;
+            decition_gps->brake = 6;
+            decition_gps->accelerator = -0.5;
+            decition_gps->wheel_angle = 0;
+            decition_gps->torque = 0;
+            decition_gps->mode = 0;
+            //decitionExecuter->executeDecition(decition_gps);
+                   ShareDecition(decition_gps);  //apollo_fu 20200413 qudiaozhushi
+            ServiceCarStatus.mfAcc = decition_gps->accelerator;
+            ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
+            ServiceCarStatus.mfBrake = decition_gps->brake;
+
+            iv::brain::brainstate xbs;
+            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
+            xbs.set_mbbrainrunning(false);
+            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
+            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
+            xbs.set_mfobs(ServiceCarStatus.mObs);
+            xbs.set_decision_period(decision_period);
+            ShareBrainstate(xbs);    //apollo_fu 20200413 qudiaozhushi
+            //            iv::decition::VehicleStateBasic vsb;
+            //            vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
+            //            vsb.mfLidarObs = ServiceCarStatus.mLidarObs;
+            //            vsb.mfRadarObs = ServiceCarStatus.mRadarObs;
+            //            vsb.mfObs = ServiceCarStatus.mRadarObs;
+            //            vsb.decision_period = decision_period;
+            //            vsb.mbBrainRunning = false;
+            ////             mpaVechicleState->writemsg((char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+            //            iv::modulecomm::ModuleSendMsg(mpaVechicleState,(char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+
+#ifdef Q_OS_LINUX
+            usleep(10000);
+#endif
+#ifdef Q_OS_WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+#endif
+             std::cout<<"enter mbRunPause"<<std::endl;
+            continue;
+
+        }
+
+
+
+        ServiceCarStatus.mbBrainCtring = true;
+        obs_lidar_ = obs_radar_ = NULL;
+
+        int j;
+        //      gps_data_ = NULL;
+
+        //        if(obs_lidar_grid_ != NULL)free(obs_lidar_grid_);
+//        if(fusion_ptr_ != NULL)
+//        {
+//            if(old_obs_lidar_grid_ != NULL) free(old_obs_lidar_grid_);
+//            old_obs_lidar_grid_ = fusion_ptr_;
+//        }
+
+//        fusion_ptr_ = NULL;
+
+        if(obs_camera_ != NULL)free(obs_camera_);
+
+        obs_camera_ = NULL;
+
+
+
+        eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
+
+/*
+        if(obs_lidar_grid_!= NULL)
+        {
+            std::cout<<"receive fusion date"<<std::endl;
+            int i,j;
+            for(i=0;i<iv::grx;i++)
+            {
+                for(j=0;j<iv::gry;j++)
+                {
+                 if(obs_lidar_grid_[i*iv::gry+j].ob == 2){
+                     std::cout<<"ok"<<std::endl;
+                     if(obs_lidar_grid_[i*iv::gry+j].id > 10)
+                     {
+                         int xx = obs_lidar_grid_[i*iv::gry+j].id;
+                         xx++;
+                     }
+                      std::cout<<obs_lidar_grid_[i*iv::gry+j].id<<std::endl;
+                 }
+
+
+                }
+            }
+              std::cout<<"print fusion date end"<<std::endl;
+        }
+*/
+
+        ServiceLidar.copylidarperto(lidar_per);
+
+//                if(lidar_per->size() >0)
+//                {
+//                    int i;
+//                    for(i=0;i<lidar_per->size();i++)
+//                    {
+//                        if(lidar_per->at(i).label>0)
+//                        {
+//                            std::cout<<"label is"<<lidar_per->at(i).label<<" vy = "<<lidar_per->at(i).velocity.y<<std::endl;
+//                        }
+//                    }
+//                    //if size > 0, have perception result;
+//                }
+
+
+                if(lidar_per->empty()){
+                   miss_lidar_per_count++;
+                   miss_lidar_per_count=min(miss_lidar_per_limit,miss_lidar_per_count);
+                   if(miss_lidar_per_count<miss_lidar_per_limit && old_lidar_per!=NULL){
+                    lidar_per=old_lidar_per;
+                   }
+                }else{
+                    old_lidar_per=lidar_per;
+                    miss_lidar_per_count=0;
+                }
+
+//                if(lidar_per->size() >0)
+//                {
+//                    int i;
+//                    for(i=0;i<lidar_per->size();i++)
+//                    {
+//                        if(lidar_per->at(i).label>0)
+//                        {
+//                            std::cout<<"label is"<<lidar_per->at(i).label<<" vy = "<<lidar_per->at(i).velocity.y<<std::endl;
+//                        }
+//                    }
+//                    //if size > 0, have perception result;
+//                }
+
+        iv::ObsLiDAR obs_lidar_tmp(new std::vector<iv::ObstacleBasic>);
+        iv::ObsRadar obs_radar_tmp(new std::vector<iv::ObstacleBasic>);
+        iv::decition::Decition decition_final(new iv::decition::DecitionBasic);
+
+        //		if ((ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == false && ServiceCarStatus.status[4] == true) ||
+        //			(ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true) ||
+        //			(ServiceCarStatus.status[0] == true && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == true && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
+        //		{
+        //			//停车
+        //			ServiceCarStatus.carState = 0;
+        //		}
+        //		else if ((ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == false && ServiceCarStatus.status[4] == true) ||
+        //				 (ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
+        //		{
+        //			//正常循迹
+        //			ServiceCarStatus.carState = 1;
+        //		}
+        //		else if ((ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == false && ServiceCarStatus.status[2] == false && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true) ||
+        //				 (ServiceCarStatus.status[0] == false && ServiceCarStatus.status[1] == true && ServiceCarStatus.status[2] == true && ServiceCarStatus.status[3] == true && ServiceCarStatus.status[4] == true))
+        //		{
+        //			//前往站点
+        //			ServiceCarStatus.carState = 2;
+        //		}
+
+        if (obs_lidar_) {	//激光雷达有障碍物
+            //          std::cout<<"obs lidar size = "<<obs_lidar_->size()<<std::endl;
+            //std::cout << "大脑处理激光雷达:" << obs_lidar_->at(0).nomal_x << " " << obs_lidar_->at(0).nomal_y << " " << obs_lidar_->at(0).nomal_z << std::endl;
+            obs_lidar_tmp = NULL;
+            obs_lidar_tmp.swap(obs_lidar_);
+        }
+        else
+        {
+            //           std::cout<<"no lidar obs"<<std::endl;
+        }
+        //ServiceCarStatus.obs_radar;
+        //毫米波雷达障碍物信息
+
+        if (obs_radar_) {
+            //std::cout << "大脑处理毫米波雷达:" << obs_radar_->at(0).nomal_x << " " << obs_radar_->at(0).nomal_y << " " << obs_radar_->at(0).nomal_z << std::endl;
+            obs_radar_tmp = NULL;
+            obs_radar_tmp.swap(obs_radar_);
+        }
+        if (obs_camera_) {
+            //std::cout << "大脑处理摄像头雷达:" << obs_camera_->at(0).nomal_x << " " << obs_camera_->at(0).nomal_y << " " << obs_camera_->at(0).nomal_z << std::endl;
+        }
+
+
+        //useMobileEye chuku
+        if(ServiceCarStatus.m_bOutGarage){
+            GPS_INS gps ;
+            if(gps_data_){
+                gps=*gps_data_;
+            }
+            decition_gps = decitionLine00->getDecideFromGPS(gps, navigation_data, obs_lidar_grid_,*lidar_per);  //chedaoxian
+
+            std::cout << "使用mobileEye决策"<< std::endl;
+        }
+        //useMobileEye chuku end;
+
+
+
+
+
+
+
+        if (gps_data_  &&!handPark && !ServiceCarStatus.m_bOutGarage){// && obs_lidar_grid_) {	//处理GPS数据
+            //todo gps_data_为当前读到的gps位置信息
+            //decition_gps = iv::decition::Decition(new iv::decition::DecitionBasic);
+            //ODS("接收GPS数据:%f\t\t%f\n", gps_data_->gps_x, gps_data_->gps_y);
+            //	decition_gps = decitionMaker->getDecideForGPS(*gps_data_,navigation_data);
+
+            //ODS("\n接收到的实时GPS数据:%f\t%f\t%f\n", gps_data_->gps_x, gps_data_->gps_y, gps_data_->ins_heading_angle);
+
+            //以下为正常用
+            //         if (gps_data_->valid == RTK_IMU_OK)
+            //        {
+            double start = GetTickCount();
+
+            //				decitionGps00->aim_gps_ins.gps_lat = carcall->lat;
+            //				decitionGps00->aim_gps_ins.gps_lng = carcall->lon;
+            /*iv::ObsRadar obs_radar_tmp(new std::vector<iv::ObstacleBasic>);
+                if(obs_radar_){
+                    obs_radar_tmp = NULL;
+                    obs_radar_tmp.swap(obs_radar_);
+                }*/
+            //decition_gps = decitionGpsUnit->getDecideFromGPS(*gps_data_, navigation_data, *obs_radar_tmp, obs_lidar_grid_);
+            //		decition_gps = decitionGpsUnit->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);     // 新的
+
+
+            if ( ServiceCarStatus.epsMode==0 && ServiceCarStatus.receiveEps) {
+                ServiceCarStatus.mbRunPause=true;
+
+            }
+            ServiceCarStatus.receiveEps=false;
+
+            lastMode=ServiceCarStatus.epsMode;
+            lastPause=ServiceCarStatus.mbRunPause;
+
+            if((fabs(mTime.elapsed() - mnOldTime)>40))
+            {
+                if((fabs(mTime.elapsed() - mnOldTime)<10000))
+                {
+                    ServiceCarStatus.mfCalAcc = (ServiceCarStatus.speed - mfSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
+                    ServiceCarStatus.mfVehAcc = (ServiceCarStatus.vehSpeed_st - mfVehSpeedLast)*1000/(fabs(mTime.elapsed() - mnOldTime))/3.6;
+                }else
+                {
+                    ServiceCarStatus.mfCalAcc = 0;
+                    ServiceCarStatus.mfVehAcc = 0;
+                }
+                mnOldTime = mTime.elapsed();
+                mfSpeedLast = ServiceCarStatus.speed;
+                mfVehSpeedLast=ServiceCarStatus.vehSpeed_st;
+            }
+
+//            iv::LidarGridPtr templidar= obs_lidar_grid_;
+            iv::LidarGridPtr  templidar;
+            templidar = NULL;
+            mMutex_.lock();
+
+            if(fusion_ptr_ != NULL)
+            {
+               if(old_obs_lidar_grid_ != NULL)
+               {
+                   free(old_obs_lidar_grid_);
+               }
+                old_obs_lidar_grid_ = fusion_ptr_;
+                templidar = fusion_ptr_;
+
+            }
+            fusion_ptr_ = NULL;
+            mMutex_.unlock();
+
+//            iv::LidarGridPtr  templidar = fusion_ptr_;
+
+
+            if(templidar == NULL)
+                templidar = old_obs_lidar_grid_;
+
+
+        //    GaussProjCal(gps_data_->gps_lng,gps_data_->gps_lat,&gps_data_->gps_x,&gps_data_->gps_y);
+
+
+
+//            if(templidar!= NULL)
+//            {
+//                std::cout<<"receive fusion date"<<std::endl;
+//                int i,j;
+//                for(i=0;i<iv::grx;i++)
+//                {
+//                    for(j=0;j<iv::gry;j++)
+//                    {
+//                     if(templidar[i*iv::gry+j].ob == 2){
+//                         std::cout<<"ok"<<std::endl;
+//                         if(templidar[i*iv::gry+j].id > 10)
+//                         {
+//                             int xx = templidar[i*iv::gry+j].id;
+//                             xx++;
+//                         }
+//                          std::cout<<templidar[i*iv::gry+j].id<<std::endl;
+//                     }
+
+
+//                    }
+//                }
+//                  std::cout<<"print fusion date end"<<std::endl;
+//            }
+
+
+
+
+
+            mMutexMap.lock();
+            decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================
+//            free(templidar);
+            mMutexMap.unlock();                  
+            if(mbUseExternMPC)
+            {
+                fanya::GPS_INS xgpsins;
+                xgpsins.speed_y = gps_data_->speed/3.6;
+                xgpsins.gps_lat = gps_data_->gps_lat;
+                xgpsins.gps_lng = gps_data_->gps_lng;
+                xgpsins.ins_heading = gps_data_->ins_heading_angle;
+                mmpcapi.SetGPS(xgpsins);
+                mmpcapi.SetDesiredspeed(decition_gps->speed/3.6);
+                double mpc_speed,mpc_decision,mpc_wheel;
+                if(mmpcapi.GetDecision(mpc_speed,mpc_decision,mpc_wheel) == 0)
+                {
+                    decition_gps->wheel_angle = mpc_wheel;
+                }
+            }
+
+            int ntpsize = sizeof(iv::TracePoint);
+            iv::modulecomm::ModuleSendMsg(mpaPlanTrace,(char *)decitionGps00->planTrace.data(),decitionGps00->planTrace.size()*sizeof(iv::TracePoint));
+
+            iv::modulecomm::ModuleSendMsg(mpaObsTraceLeft,(char *)(ServiceCarStatus.obsTraceLeft.data()),ServiceCarStatus.obsTraceLeft.size()*sizeof(iv::TracePoint));
+            iv::modulecomm::ModuleSendMsg(mpaObsTraceRight,(char *)(ServiceCarStatus.obsTraceRight.data()),ServiceCarStatus.obsTraceRight.size()*sizeof(iv::TracePoint));
+
+
+            //      decition_gps = decitionLine00->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);
+            //ODS("\n决策：%f\t%f\t\t", decition_gps->speed, decition_gps->wheel_angle);
+            //ODS("\n接收到的实时GPS数据:%f\t%f\t%f\n", gps_data_->gps_x, gps_data_->gps_y,gps_data_->ins_heading_angle);
+
+            //carcall->is_arrived = decitionGps00->is_arrivaled;
+            //double end = GetTickCount();
+            decision_period = start - last;
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n周期时长:%f\n" << decision_period << std::endl;
+            //                iv::decition::VehicleStateBasic vsb;
+            //                vsb.mbBocheEnable = ServiceCarStatus.bocheEnable;
+            //                vsb.mfLidarObs = ServiceCarStatus.mLidarObs;
+            //                vsb.mfRadarObs = ServiceCarStatus.mRadarObs;
+            //                vsb.mfObs = ServiceCarStatus.mRadarObs;
+            //                vsb.decision_period = decision_period;
+            //                vsb.mbBrainRunning = ServiceCarStatus.mbBrainCtring;
+            ////                mpaVechicleState->writemsg((char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+
+            //                iv::modulecomm::ModuleSendMsg(mpaVechicleState,(char *)&vsb,sizeof(iv::decition::VehicleStateBasic));
+
+            iv::brain::brainstate xbs;
+            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
+            xbs.set_mbbrainrunning(true);  //apollo_fu debug ui 20200417
+            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
+            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
+            xbs.set_mfobs(ServiceCarStatus.mObs);
+            xbs.set_decision_period(decision_period);
+            ShareBrainstate(xbs);
+
+
+            //                decitionExecuter->executeDecition(decition_gps);
+            ShareDecition(decition_gps);
+
+
+
+            iv::brain::carstate car_xbs;
+            car_xbs.set_mstate(ServiceCarStatus.mvehState);
+            car_xbs.set_mavoidx(ServiceCarStatus.mavoidX);
+            car_xbs.set_mobsdistance(ServiceCarStatus.mObsDistance);
+            car_xbs.set_mobsspeed(ServiceCarStatus.mObsSpeed);
+            ShareBraincarstate(car_xbs);
+
+            givlog->debug("acc is %f brake is %f wheel is %f",
+                          decition_gps->accelerator,decition_gps->brake,
+                          decition_gps->wheel_angle);
+            givlog->debug("period id %f",decision_period);
+
+            //	ODS("\n周期时长:%f\n", start - last);
+            //	ODS("\n决策时长:%f\n", end - start);
+            //	ODS("\n接收到的实时GPS车速:%f\n", gps_data_->speed);
+            //	ODS("\n接收到的实时GPS车y轴加速:%f\n", gps_data_->accel_y);
+            //	ODS("\n接收到的实时GPS车x轴加速:%f\n", gps_data_->accel_x);
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策加速度:%f\n" << decition_gps->accelerator << std::endl;
+            //		ODS("\n决策刹车:%f\n", decition_gps->brake);
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策方向:%f\n" << decition_gps->wheel_angle << std::endl;
+            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "\n决策速度:%f\n" << decition_gps->speed << std::endl;
+//            double decition_period=GetTickCount()-start;
+//            givlog->debug("decition_brain","period: %f",
+//                            decition_period);
+            last = start;
+            //decitionMaker->decideFromGPS(decition_gps, gps_data_, navigation_data);		//gps_data_为当前读到的gps位置信息  navigation_data为导航数据  decition_gps为根据前两者得出的决策速度与决策角度
+            //            }
+
+        }
+
+        if (handPark && decition_gps != NULL)
+        {
+            decition_gps->brake = 20;
+            decition_gps->accelerator = 0;
+            //			decitionExecuter->executeDecition(decition_gps);
+            ShareDecition(decition_gps);
+            //boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+
+#ifdef linux
+            usleep(2000000);
+#endif
+#ifdef WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
+            //           Sleep(2000);
+#endif
+            decition_gps->brake = 0;
+            decition_gps->accelerator = 0;
+            //			decitionExecuter->executeDecition(decition_gps);
+            //			ServiceCanUtil.set_handbrake_on();
+            //boost::this_thread::sleep(boost::posix_time::milliseconds(handParkTime));
+            ShareDecition(decition_gps);
+
+#ifdef linux
+            usleep(handParkTime*1000);
+#endif
+#ifdef WIN32
+            Sleep(handParkTime);
+#endif
+            handPark = false;
+        }
+
+
+        else
+        {
+            //boost::this_thread::sleep(boost::posix_time::milliseconds(1));
+
+#ifdef linux
+            usleep(1000);
+#endif
+#ifdef WIN32
+            Sleep(1);
+#endif
+            //			ODS("no gps data .sleep.\r\n");
+        }
+
+
+
+
+        //        if (decition_lidar || decition_radar || decition_camera || decition_gps || decition_lidar) {
+        //            //	decitionVoter->decideFromAll(decition_final, decition_lidar, decition_radar, decition_camera, decition_gps, decition_localmap);
+        //            //	decitionExecuter->executeDecition(decition_final);	//执行最终的决策
+        //            /*	decition_gps->brake = 0;
+        //            decition_gps->accelerator = 0;*/
+        //            look1 = decition_gps->accelerator;
+        //            look2 = decition_gps->brake;
+        //            look3 = decition_gps->wheel_angle;
+        //            look4 = decition_gps->speed;
+        //            look5 = decitionGps00->lidarDistance;
+        //            look6 = decitionGps00->myesrDistance;
+        //            look7 = decitionGps00->obsDistance;
+
+        //            decition_gps->grade=1;
+        //            decition_gps->mode=1;
+        //            decition_gps->speak=0;
+        //            decition_gps->handBrake=0;
+        //            decition_gps->bright=false;
+        //            decition_gps->engine=0;
+
+        //            if(ServiceCarStatus.bocheMode){
+        //                decition_gps->dangWei=2;
+        //            }else{
+        //                decition_gps->dangWei=1;
+        //            }
+
+
+        //            decition_gps->wheel_angle=max((float)-500.0,float(decition_gps->wheel_angle+ServiceCarStatus.mfEPSOff));
+        //            decition_gps->wheel_angle=min((float)500.0,decition_gps->wheel_angle);
+
+        //            //			decitionExecuter->executeDecition(decition_gps);
+        //            //           ShareDecition(decition_gps);
+        //        }
+
+        ServiceCarStatus.mfAcc = decition_gps->accelerator;
+        ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
+        ServiceCarStatus.mfBrake = decition_gps->brake;
+
+        iv::platform::PlatFormMsg toplat;
+        toplat.carState = ServiceCarStatus.carState;	// 0:停车	1:正常循迹	2:前往站点
+        toplat.istostation = ServiceCarStatus.istostation;
+        toplat.currentstation = ServiceCarStatus.currentstation;
+        toplat.clientto = ServiceCarStatus.clientto;
+        toplat.amilng = ServiceCarStatus.amilng;
+        toplat.amilat = ServiceCarStatus.amilat;
+        //       mpaToPlatform->writemsg((char*)&toplat,sizeof(iv::platform::PlatFormMsg));
+
+        iv::modulecomm::ModuleSendMsg(mpaToPlatform,(char*)&toplat,sizeof(iv::platform::PlatFormMsg));
+        //ODS("\ngetSensor时长:%f\n", end1 - start1);
+    }
+    std::cout << "\n\n\n\n\n\n\n\n\n\n\nbrain线程退出\n" << std::endl;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            //      gps2xy(data->gps_lat, data->gps_lng, &data->gps_x, &data->gps_y);
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            //data->speed = 5;	//调试用  所有点速度都设为5km/h
+            //LOG2(data->gps_x, data->gps_y);
+            //ODS("x %.20lf   y %.20lf\n", data->gps_x, data->gps_y);
+
+            //	iv::decition::BLH2XYZ(*data);   //重新转下大地坐标
+
+            if(data->roadMode==4){
+                data->ins_heading_angle=data->ins_heading_angle+180;
+                if(data->ins_heading_angle>=360)
+                    data->ins_heading_angle=data->ins_heading_angle-360;
+            }
+
+            navigation_data.push_back(data);
+        }
+
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile2(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            navigation_data2.push_back(data);
+        }
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile3(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            navigation_data3.push_back(data);
+        }
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+bool iv::decition::BrainDecition::loadMapFromFile4(std::string fileName) {
+    std::ifstream fis(fileName);//获取文件
+    std::string line;
+    std::vector<std::string> des;
+
+    if (fis.is_open() == false)
+        return false;
+    try {
+        while (std::getline(fis, line)) {//开始一行一行的读数据
+            boost::split(des, line, boost::is_any_of("\t"));
+            if (des.size() != 10)
+                throw "error";
+            iv::GPSData data(new iv::GPS_INS);
+            data->index = atoi(des[0].c_str());
+            data->gps_lng = atof(des[1].c_str());
+            data->gps_lat = atof(des[2].c_str());
+            data->speed_mode = atoi(des[3].c_str());
+            data->mode2 = atoi(des[4].c_str());
+            data->ins_heading_angle = atof(des[5].c_str());
+            data->runMode = atoi(des[6].c_str());
+            data->roadMode = atoi(des[7].c_str());
+            data->roadSum = atoi(des[8].c_str());
+            data->roadOri = atoi(des[9].c_str());
+            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
+            navigation_data4.push_back(data);
+        }
+    }
+    catch (...) {
+        fis.close();
+        return false;
+    }
+
+    fis.close();
+    return true;
+}
+
+void iv::decition::BrainDecition::start() {
+    thread_run = new boost::thread(boost::bind(&iv::decition::BrainDecition::run, this));
+}
+
+void iv::decition::BrainDecition::stop() {
+    //    carcall->stop();
+    thread_run->interrupt();
+    thread_run->join();
+}
+
+
+void iv::decition::BrainDecition::SideMove(iv::GPS_INS &x)
+{
+    if(ServiceCarStatus.msysparam.mbSideDrive == false)return;
+    double fhdg = (90 - x.ins_heading_angle)*M_PI/180.0;
+    double rel_x,rel_y;
+    GaussProjCal(x.gps_lng,x.gps_lat,&rel_x,&rel_y);
+    double fmove = 0;
+    fmove = ServiceCarStatus.msysparam.mfSideDis + ServiceCarStatus.msysparam.vehWidth/2.0 - x.mfLaneWidth +x.mfDisToLaneLeft;
+    if(fmove == 0.0)return;
+    double rel_x1,rel_y1;
+    rel_x1 = rel_x + fmove * cos(fhdg + M_PI/2.0);
+    rel_y1 = rel_y + fmove * sin(fhdg + M_PI/2.0);
+    GaussProjInvCal(rel_x1,rel_y1,&x.gps_lng,&x.gps_lat);
+    x.gps_x = rel_x1;
+    x.gps_y = rel_y1;
+}
+
+void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapdatasize)
+{
+    //    std::cout<<"update map "<<std::endl;
+    int gpsunitsize = sizeof(iv::GPS_INS);
+    int nMapSize = mapdatasize/gpsunitsize;
+    //    std::cout<<"map size is "<<nMapSize<<std::endl;
+
+    if(nMapSize < 1)return;
+
+    bool bUpdate = false;
+    if(nMapSize != navigation_data.size())
+    {
+        bUpdate = true;
+    }
+    else
+    {
+        iv::GPS_INS * p = (iv::GPS_INS *)mapdata;
+        if((p->gps_lat == navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == navigation_data.at(0)->ins_heading_angle))
+        {
+            //           qDebug("same map");
+            bUpdate = false;
+        }
+        else
+        {
+            bUpdate = true;
+        }
+    }
+
+    if(bUpdate)
+    {
+        std::vector<fanya::MAP_DATA> xvectorMAP;
+        int i;
+        mMutexMap.lock();
+        navigation_data.clear();
+        for(i=0;i<nMapSize;i++)
+        {
+            iv::GPS_INS x;
+            memcpy(&x,mapdata + i*gpsunitsize,gpsunitsize);
+            iv::GPSData data(new iv::GPS_INS);
+            if(ServiceCarStatus.msysparam.mbSideDrive)SideMove(x);
+            *data = x;
+            navigation_data.push_back(data);
+
+   //         if(data->mode2 > 0)
+     //       {
+      //          int a = 1;
+       //     }
+
+            fanya::MAP_DATA xmapdata;
+            xmapdata.gps_lat = x.gps_lat;
+            xmapdata.gps_lng = x.gps_lng;
+            xmapdata.ins_heading = x.ins_heading_angle;
+            xvectorMAP.push_back(xmapdata);
+        }
+
+        mmpcapi.SetMAP(xvectorMAP);
+
+//        if(ServiceCarStatus.speed_control==true){
+//        Compute00().getDesiredSpeed(navigation_data);
+//            Compute00().getPlanSpeed(navigation_data);
+//        }
+        mMutexMap.unlock();
+        //        mpasd->SaveState("map",mapdata,mapdatasize);
+    }
+    else
+    {
+        //       qDebug("not need update");
+    }
+}
+
+void iv::decition::BrainDecition::ShareDecition(iv::decition::Decition decition)
+{
+    iv::brain::decition xdecition;
+    xdecition.set_accelerator(decition->accelerator);
+    xdecition.set_brake(decition->brake);
+    xdecition.set_leftlamp(decition->leftlamp);
+    xdecition.set_rightlamp(decition->rightlamp);
+    xdecition.set_speed(decition->speed);
+    xdecition.set_wheelangle(decition->wheel_angle);
+    xdecition.set_wheelspeed(decition->angSpeed);
+    xdecition.set_torque(decition->torque);
+    xdecition.set_mode(decition->mode);
+    xdecition.set_gear(decition->dangWei);
+    xdecition.set_handbrake(decition->handBrake);
+    xdecition.set_grade(decition->grade);
+    xdecition.set_engine(decition->engine);
+    xdecition.set_brakelamp(decition->brakeLight);
+    xdecition.set_ttc(ServiceCarStatus.mfttc);
+//    xdecition.set_air_enable(decition->air_enable);
+//    xdecition.set_air_temp(decition->air_temp);
+//    xdecition.set_air_mode(decition->air_mode);
+//    xdecition.set_wind_level(decition->wind_level);
+    xdecition.set_roof_light(decition->roof_light);
+    xdecition.set_home_light(decition->home_light);
+//    xdecition.set_air_worktime(decition->air_worktime);
+//    xdecition.set_air_offtime(decition->air_offtime);
+//    xdecition.set_air_on(decition->air_on);
+    xdecition.set_door(decition->door);
+    xdecition.set_dippedlight(decition->nearLight);
+
+    std::cout<<"===================shareDecition========================"<<std::endl;
+         std::cout<<"  torque_st:"<<ServiceCarStatus.torque_st
+                 <<"  decideTorque:"<<decition->torque <<"  decideBrake:"<<decition->brake
+                <<"  decition mode:"<<decition->mode
+        <<std::endl;
+    std::cout<<"========================================="<<std::endl;
+
+//    givlog->verbose("torque_st: %ld",ServiceCarStatus.torque_st);
+//     givlog->verbose("decideTorque: %ld",decition->torque);
+//     givlog->verbose("decideBrake: %ld", decition->brake);
+//     givlog->debug("wheelAng: %f",decition->wheel_angle);
+
+   //  xdecition.set_wheelangle(100);
+    static qint64 oldtime;
+
+    if((QDateTime::currentMSecsSinceEpoch() - oldtime)>100)
+    {
+//        givlog->warn("brain interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldtime);
+    }
+
+    //givlog->verbose("decition time is %ld",QDateTime::currentMSecsSinceEpoch());
+    oldtime = QDateTime::currentMSecsSinceEpoch();
+
+
+    //givlog->verbose("torque %f wheel %f dangwei %d ",decition->torque,
+                    //decition->wheel_angle,decition->dangWei);
+    int nsize = xdecition.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(xdecition.SerializeToArray(str,nsize))
+    {
+        if(ServiceCarStatus.mbRunPause == false)
+        {
+
+            iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
+        }
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareDecition serialize error."<<std::endl;
+    }
+}
+
+void iv::decition::BrainDecition::ShareBrainstate(iv::brain::brainstate xbs)
+{
+    int nsize = xbs.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(xbs.SerializeToArray(str,nsize))
+    {
+        iv::modulecomm::ModuleSendMsg(mpaVechicleState,str,nsize);
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
+    }
+}
+
+void iv::decition::BrainDecition::ShareBraincarstate(iv::brain::carstate xbs)
+{
+    int nsize = xbs.ByteSize();
+    char * str = new char[nsize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(xbs.SerializeToArray(str,nsize))
+    {
+        iv::modulecomm::ModuleSendMsg(mpaCarstate,str,nsize);
+    }
+    else
+    {
+        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
+    }
+}
+
+
+
+void iv::decition::BrainDecition::UpdateHMI(const char *pdata, const int ndatasize)
+{
+    if(ndatasize < sizeof(iv::hmi::HMIBasic))return;
+
+    iv::hmi::hmimsg xhmimsg;
+    if(!xhmimsg.ParseFromArray(pdata,ndatasize))
+    {
+        givlog->error("iv::decition::BrainDecition::UpdateHMI parse error");
+        return;
+    }
+
+    ServiceCarStatus.mbRunPause = xhmimsg.mbpause();
+
+    if(xhmimsg.mbbochemode()){
+         ServiceCarStatus.bocheMode = true;
+    }
+    ServiceCarStatus.busmode = xhmimsg.mbbusmode();
+
+    if( ServiceCarStatus.bocheMode ){
+        int a=0;
+    }
+
+    bool bRun;
+    bRun = !ServiceCarStatus.mbRunPause;
+
+    //    mpasd->SaveState("runstate",(char *)&bRun,sizeof(bool));
+
+
+
+    ServiceCarStatus.has_mbdoor = xhmimsg.has_mbchemen();
+    if(ServiceCarStatus.has_mbdoor){
+        ServiceCarStatus.mbdoor = xhmimsg.mbchemen();
+
+    }
+
+    ServiceCarStatus.has_mbjinguang = xhmimsg.has_mbjinguang();
+    if(ServiceCarStatus.has_mbjinguang){
+        ServiceCarStatus.mbjinguang = xhmimsg.mbjinguang();
+    }
+
+    givlog->debug("decition_brain_bool","mbdoor: %d,mbjinguang: %d",
+                    ServiceCarStatus.mbdoor,ServiceCarStatus.mbjinguang);
+
+
+}
+
+void iv::decition::BrainDecition::UpdatePlatform(const char *pdata, const int ndatasize)
+{
+    if(ndatasize < sizeof(iv::platform::PlatFormMsg))return;
+
+    iv::platform::PlatFormMsg x;
+    memcpy(&x,pdata,sizeof(iv::platform::PlatFormMsg));
+    ServiceCarStatus.carState = x.carState;	// 0:停车	1:正常循迹	2:前往站点
+    std::cout<<"car state "<<ServiceCarStatus.carState<<std::endl;
+    ServiceCarStatus.istostation = x.istostation;
+    ServiceCarStatus.currentstation = x.currentstation;
+    ServiceCarStatus.clientto = x.clientto;
+    ServiceCarStatus.amilng = x.amilng;
+    ServiceCarStatus.amilat = x.amilat;
+
+}
+
+void iv::decition::BrainDecition::UpdateGRPCGroupMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    iv::group::groupinfo xgroupinfo;
+    if(!xgroupinfo.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"iv::decition::BrainDecition::UpdateGRPCGroupMsg parse fail."<<std::endl;
+        return;
+    }
+
+    int nvehsize = xgroupinfo.mvehinfo_size();
+
+    mMutexGroupgrpc.lock();
+    mnGroupgrpcUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mgroupgrpcInfo.CopyFrom(xgroupinfo);
+
+    mMutexGroupgrpc.unlock();
+
+    ServiceCarStatus.mMutexgroupgrpc.lock();
+    ServiceCarStatus.mgroupgrpcupdatetime = QDateTime::currentMSecsSinceEpoch();
+    ServiceCarStatus.mgroupgrpcinfo.CopyFrom(xgroupinfo);
+    ServiceCarStatus.mMutexgroupgrpc.unlock();
+
+
+}
+
+void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    iv::chassis xchassis;
+    static int ncount = 0;
+    if(!xchassis.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
+        return;
+    }
+
+    ServiceCarStatus.epb = xchassis.epbfault();
+    ServiceCarStatus.grade = xchassis.shift();
+    ServiceCarStatus.driverMode = xchassis.drivemode();
+    if(xchassis.has_epsmode()){
+        ServiceCarStatus.epsMode = xchassis.epsmode();
+        ServiceCarStatus.receiveEps=true;
+        if(ServiceCarStatus.epsMode == 0)
+        {
+            ServiceCarStatus.mbRunPause = true;
+        }
+    }
+    if(xchassis.has_torque())
+    {
+         ServiceCarStatus.torque_st = xchassis.torque();
+        if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+            ServiceCarStatus.torque_st = xchassis.torque()*0.4;
+        }
+        std::cout<<"******* ServiceCarStatus.torque_st:"<< ServiceCarStatus.torque_st<<std::endl;
+        givlog->warn(" ServiceCarStatus.torque_st: is %f",ServiceCarStatus.torque_st);
+
+    }
+
+    if(xchassis.has_engine_speed())
+    {
+         ServiceCarStatus.engine_speed = xchassis.engine_speed();
+
+         std::cout<<"******* xchassis.engine_speed:"<< xchassis.engine_speed()<<std::endl;
+
+
+    }
+
+
+    //    if(xchassis.epsmode() == 1)
+    //    {
+    //        ncount++;
+    ////        if(ncount > 10)ServiceCarStatus.mbRunPause = true;
+    //    }
+    //    else
+    //    {
+    //        ncount = 0;
+    //    }
+}
+
+void iv::decition::BrainDecition::GetFusion(const char *pdata, const int ndatasize)
+{
+    std::shared_ptr<iv::fusion::fusionobjectarray> xfusion (new iv::fusion::fusionobjectarray);
+
+    if(ndatasize<1)return;
+    if(false == xfusion->ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<" Listenesrfront fail."<<std::endl;
+        return;
+    }
+    else{
+        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
+    }
+
+    iv::decition::BrainDecition::UpdateFusion(xfusion);
+}
+
+void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
+{
+
+    mMutex_.lock();
+    fusion_obs.swap(mfusion_obs_);
+    if(fusion_ptr_ != NULL)
+    {
+        free(fusion_ptr_);
+        fusion_ptr_ = NULL;
+    }
+    fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+    memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));
+    for(int i =0; i<iv::grx; i++)     //复制到指针数组
+    {
+        for(int j =0; j<iv::gry; j++)
+        {
+            fusion_ptr_[i*(iv::gry)+j].ob = 0;
+        }
+
+    }
+
+    for(int i =0; i<mfusion_obs_->obj_size();i++)
+    {
+        iv::fusion::fusionobject xobs = mfusion_obs_->obj(i);
+        for(int j =0; j<xobs.nomal_centroid().size(); j++)
+        {
+            int dx, dy;
+            dx = (xobs.nomal_centroid(j).nomal_x() + gridwide*(double)centerx)/gridwide;
+            dy = (xobs.nomal_centroid(j).nomal_y() + gridwide*(double)centery)/gridwide;
+
+
+            if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
+            {
+                fusion_ptr_[dx*(iv::gry) + dy].id = xobs.id();
+                fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
+                fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().x();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().y();
+                fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
+                fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
+                fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
+
+//                if(xobs.centroid().y() < 50 && xobs.centroid().y() >5 )
+//                {
+//                    std::cout<<"     x       y    vy "<<xobs.centroid().x() <<  "     "
+//                            << xobs.centroid().y()<< "     "<<fusion_ptr_[dx*(iv::gry) + dy].speed_y<< "     "
+//                            <<xobs.vel_relative().y()<<std::endl;
+//                    givlog->debug("brain_decition","SendobsDistance: %f,SendobsSpeed: %f,objwidth: %f,nsize: %f,objsize: %f",
+//                                  xobs.centroid().y(),fusion_ptr_[dx*(iv::gry) + dy].speed_y,xobs.dimensions().x(),
+//                            xobs.nomal_centroid().size(),mfusion_obs_->obj_size());
+
+//                }
+            }
+        }
+
+    }
+    mMutex_.unlock();
+
+}
+
+
+void iv::decition::BrainDecition::UpdateGroupInfo(const char *pdata, const int ndatasize){
+
+    iv::radio::radio_send group_message;
+    if(false == group_message.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Listencansend Parse fail."<<std::endl;
+        return;
+    }
+    ServiceCarStatus.group_server_status = group_message.server_status();
+    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
+    ServiceCarStatus.group_state= group_message.cmd_mode();
+
+    qDebug("updateGroupInfo %ld",QDateTime::currentMSecsSinceEpoch());
+    if(ServiceCarStatus.group_state!=2){
+        ServiceCarStatus.group_comm_speed=0;
+    }
+    qDebug("serverSt: %d, speed: %f, state: %d", ServiceCarStatus.group_server_status, ServiceCarStatus.group_comm_speed, ServiceCarStatus.group_state);
+
+
+}
+
+void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatasize){
+
+    iv::vbox::vbox group_message;
+    if(false == group_message.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Listencansend Parse fail."<<std::endl;
+        return;
+    }
+
+//  解析box信息
+//    ServiceCarStatus.group_server_status = group_message.server_status();
+//    ServiceCarStatus.group_comm_speed= (float)group_message.car_control_speed();
+//    ServiceCarStatus.group_state= group_message.cmd_mode();
+
+
+    trafficLight.leftColor=group_message.st_left();
+    trafficLight.rightColor=group_message.st_right();
+    trafficLight.straightColor=group_message.st_straight();
+    trafficLight.uturnColor=group_message.st_turn();
+    trafficLight.leftTime=group_message.time_left();
+    trafficLight.rightTime=group_message.time_right();
+    trafficLight.straightTime=group_message.time_straight();
+    trafficLight.uturnTime=group_message.time_turn();
+
+
+
+}
+
+void iv::decition::BrainDecition::UpdateSate(){
+     decitionGps00->isFirstRun=true;
+}
+
+void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
+
+    ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.radarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearRadarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearLidarGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.frontGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;
+    ServiceCarStatus.msysparam.rearGpsXiuzheng +=  ServiceCarStatus.msysparam.gpsOffset_y;
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/brain.h

@@ -0,0 +1,199 @@
+#pragma once
+/*
+* 中央决策大脑
+*/
+
+#ifndef _IV_DECITION_BRAIN_
+#define _IV_DECITION_BRAIN_
+
+#include <common/boost.h>
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+//#include <control/controller.h>
+//#include <control/can.h>
+#include <perception/eyes.h>
+#include <decition/decition_maker.h>
+//#include <decition/decition_executer.h>
+#include <decition/decition_voter.h>
+#include <decition/decide_gps_00.h>
+#include <decition/decide_line_00.h>
+#include <decition/adc_tools/compute_00.h>
+#include <perception/sensor.h>
+#include <perception/sensor_camera.h>
+#include <perception/sensor_gps.h>
+#include<perception/sensor_lidar.h>
+#include<perception/sensor_radar.h>
+//#include <server/carcalling.h>
+//#include "adcivstatedb.h"
+#include <fusionobjectarray.pb.h>
+
+//#include "decition/vehiclestate_type.h"
+#include "common/hmi_type.h"
+#include "common/platform_type.h"
+#include"common/gps_type.h"
+
+#include <QMutex>
+#include <QTime>
+#include <memory>
+
+#include "brainstate.pb.h"
+#include "decition.pb.h"
+#include "mapreq.pb.h"
+#include "chassis.pb.h"
+#include "hmi.pb.h"
+#include "radio_send.pb.h"
+#include "ivlog.h"
+#include "formation_map.pb.h"
+#include "vbox.pb.h"
+#include "carstate.pb.h"
+#include "groupmsg.pb.h"
+
+#include "fanyaapi.h"
+
+namespace iv {
+	namespace decition {
+		class BrainDecition
+		{
+		public:
+			BrainDecition();
+			~BrainDecition();
+
+
+            void inialize();/* 初始化*/
+
+
+            bool loadMapFromFile(std::string fileName);/* 加载地图*/
+			bool loadMapFromFile2(std::string fileName);
+			bool loadMapFromFile3(std::string fileName);
+			bool loadMapFromFile4(std::string fileName);
+
+
+            void start();/* 启动大脑*/
+			void stop();	//关闭
+
+			std::vector<iv::GPSData> navigation_data;	//导航数据，GPS结构体数组
+			std::vector<iv::GPSData> navigation_data2;	//导航数据，GPS结构体数组2
+			std::vector<iv::GPSData> navigation_data3;	//导航数据，GPS结构体数组3
+			std::vector<iv::GPSData> navigation_data4;	//导航数据，GPS结构体数组4
+			std::vector<iv::ObsRadar> radar_data;
+			std::vector<iv::ObsRadar> lidar_data;
+            std::vector<iv::ObsRadar> camera_data;
+		/*	std::vector<std::vector<iv::GPSData>> mapsL;
+			std::vector<std::vector<iv::GPSData>> mapsR;*/
+
+            double decision_period = 0.0;
+            double look1 = 0.0;
+            double look2 = 0.0;
+            double look3 = 0.0;
+            double look4 = 0.0;
+            double look5 = 0.0;
+            double look6 = 0.0;
+            double look7 = 0.0;
+
+            void UpdateMap(const char * mapdata,const int mapdatasize);
+		private:
+		//	iv::perception::Eyes* eyes;							//眼睛
+ //           iv::carcalling::carcalling* carcall;
+			iv::perception::Eyes* eyes;
+			iv::decition::DecitionMaker* decitionMaker;			//决策器
+			iv::decition::DecitionVoter* decitionVoter;			//决策仲裁 判断器
+//			iv::decition::DecitionExecuter* decitionExecuter;	//决策执行器
+
+            iv::decition::DecideGps00* decitionGps00;			//决策器
+              iv::decition::DecideLine00* decitionLine00;
+
+//			boost::shared_ptr<iv::control::Controller> controller;	//实际车辆控制器
+
+			boost::thread* thread_run;
+			
+			iv::ObsLiDAR obs_lidar_;		//激光雷达障碍物
+			iv::ObsRadar obs_radar_;		//毫米波雷达障碍物
+			//iv::ObsCamera obs_camera_;		//摄像头障碍物
+			iv::CameraInfoPtr obs_camera_;
+//			iv::GPSData gps_data_;			//gps 惯导数据
+			iv::LidarGridPtr obs_lidar_grid_;//激光雷达网格化
+            iv::LidarGridPtr old_obs_lidar_grid_;//激光雷达网格化
+            iv::TrafficLight trafficLight;
+            iv::Obs_grid* obs_fusion_grid_;     //fusion网格化
+
+            std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>>  old_lidar_per;
+            std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>>  tmp_lidar_per;
+            int miss_lidar_per_count=0;
+            int miss_lidar_per_limit=20;
+
+
+
+            int lastMode;
+            bool lastPause;
+
+			void run();	//实际执行逻辑
+
+            void * mpamapreq;
+            void * mpa;
+            void * mpvbox;
+            void * mpfusion;
+            QMutex mMutexMap;
+
+            void * mpaDecition;
+            void * mpaVechicleState;
+            void * mpaToPlatform;
+            void * mpaPlanTrace;
+            void * mpaObsTraceLeft;
+            void * mpaObsTraceRight;
+            void * mpaCarstate;
+
+
+            void ShareDecition(iv::decition::Decition decition);
+            void ShareBrainstate(iv::brain::brainstate xbs);
+            void ShareBraincarstate(iv::brain::carstate xbs);
+
+        private:
+            void * mpaHMI;
+            void * mpaPlatform;
+            void *mpaGroup;
+            void * mpmapChangeSig;
+            void * mpaChassis;
+            std::string mstrmemmap_index;
+
+            int mnOldTime;
+            QTime mTime;
+            double mfSpeedLast;
+            double mfVehSpeedLast;
+
+
+        public:
+            void UpdateHMI(const char * pdata,const int ndatasize);
+            void UpdatePlatform(const char * pdata,const int ndatasize);
+            void UpdateGroupInfo(const char * pdata,const int ndatasize);
+            void UpdateSate();
+            void UpdateVbox(const char * pdata,const int ndatasize);
+            void GetFusion(const char* pdata, const int ndatasize);
+            void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
+
+        private:
+            std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
+            iv::LidarGridPtr fusion_ptr_ = NULL;
+            QMutex mMutex_;
+
+
+        private:
+//            Adcivstatedb * mpasd;
+            void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+            void adjuseGpsLidarPosition();
+            void UpdateGRPCGroupMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+            void SideMove(iv::GPS_INS &x);
+
+            iv::group::groupinfo mgroupgrpcInfo;
+            qint64 mnGroupgrpcUpdateTime = 0;
+            QMutex mMutexGroupgrpc;
+            void * mapgrpcgroup;
+
+            fanyaapi mmpcapi;
+
+            bool mbUseExternMPC = false;
+        };
+
+    }
+}
+
+#endif // !_IV_DECITION_BRAIN_

src/decition/decition_brain_sf_add_frenet_plan/decition/compute_00.cpp

@@ -0,0 +1,1843 @@
+#include <decition/compute_00.h>
+#include <decition/decide_gps_00.h>
+#include <decition/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/transfer.h>
+#include <common/constants.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+//#include <control/radar_exam.h>
+#include <control/can.h>
+using namespace std;
+
+#define PI (3.1415926535897932384626433832795)
+
+iv::decition::Compute00::Compute00() {
+
+}
+iv::decition::Compute00::~Compute00() {
+
+}
+
+
+double iv::decition::Compute00::angleLimit = 700;
+double iv::decition::Compute00::lastEA = 0;
+double iv::decition::Compute00::lastEP = 0;
+double iv::decition::Compute00::decision_Angle = 0;
+double iv::decition::Compute00::lastAng = 0;
+int iv::decition::Compute00::lastEsrID = -1;
+int  iv::decition::Compute00::lastEsrCount = 0;
+int iv::decition::Compute00::lastEsrIDAvoid = -1;
+int  iv::decition::Compute00::lastEsrCountAvoid = 0;
+
+iv::GPS_INS  iv::decition::Compute00::nearTpoint;
+iv::GPS_INS  iv::decition::Compute00::farTpoint;
+double  iv::decition::Compute00::bocheAngle;
+
+
+
+iv::GPS_INS  iv::decition::Compute00::dTpoint0;
+iv::GPS_INS  iv::decition::Compute00::dTpoint1;
+iv::GPS_INS  iv::decition::Compute00::dTpoint2;
+iv::GPS_INS  iv::decition::Compute00::dTpoint3;
+double  iv::decition::Compute00::dBocheAngle;
+
+
+std::vector<int> lastEsrIdVector;
+std::vector<int> lastEsrCountVector;
+
+int iv::decition::Compute00::getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    //	DecideGps00().minDis = iv::MaxValue;
+    DecideGps00().minDis = 20;
+    DecideGps00().maxAngle = iv::MaxValue;
+
+    int startIndex = max((lastIndex - 500), 0);     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = min((gpsMap.size() - 1), (size_t)(lastIndex + 2000 + 100 * (gpsMissCount + 1)));
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < DecideGps00().minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+                )
+        {
+            index = i;
+            DecideGps00().minDis = tmpdis;
+            DecideGps00().maxAngle = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            DecideGps00().maxAngle = min(DecideGps00().maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+    /*if (index == -1) {
+        index = 0;
+    }*/
+    return index;
+}
+
+
+//首次找点
+
+int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
+{
+    int index = -1;
+    //	DecideGps00().minDis = iv::MaxValue;
+    DecideGps00().minDis = 40;
+    DecideGps00().maxAngle = iv::MaxValue;
+
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < DecideGps00().minDis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
+                                              || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
+                )
+        {
+            index = i;
+            DecideGps00().minDis = tmpdis;
+            DecideGps00().maxAngle = min(abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle), abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360));
+            DecideGps00().maxAngle = min(DecideGps00().maxAngle, abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360));
+        }
+    }
+    /*if (index == -1) {
+        index = 0;
+    }*/
+    return index;
+}
+
+
+
+
+
+double iv::decition::Compute00::getAveDef(std::vector<Point2D> farTrace)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x / average_y) / PI * 180;
+}
+
+
+
+double iv::decition::Compute00::getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX)
+{
+    double sum_x = 0;
+    double sum_y = 0;
+
+    for (int i = 0; i < min(5, (int)farTrace.size()); i++)
+    {
+        sum_x += farTrace[i].x;
+        sum_y += abs(farTrace[i].y);
+    }
+    double average_y = sum_y / min(5, (int)farTrace.size());
+    double average_x = sum_x / min(5, (int)farTrace.size());
+
+
+    return atan(average_x + avoidX / average_y) / PI * 180;
+}
+
+
+
+
+
+
+double iv::decition::Compute00::getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    //   double KEang = 14, KEPos = 10, DEang = 3, DEPos = 1;  // double KEang = 14, KEPos = 10, DEang = 10, DEPos = 10;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    if(transferPieriod&& !transferPieriod2){
+        DEang = 200;
+        DEPos = 150;
+    }
+
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+    if(changeRoad ||transferPieriod){
+        PreviewDistance=PreviewDistance+avoidX;
+    }
+    if(realSpeed <15){
+        PreviewDistance = max(4.0, realSpeed *0.4) ;
+    }
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+
+
+
+    EPos = gpsTrace[gpsIndex].x;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAveDef(farTrace);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+
+int  iv::decition::Compute00::getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed)
+{
+    int index = 1;
+    double sumdis = 0;
+    while (index < gpsTrace.size() && sumdis < realSpeed)
+        sumdis += GetDistance(gpsTrace[index - 1], gpsTrace[index++]);
+
+    if (index == gpsTrace.size())
+        return index - 1;
+
+    if (abs(sumdis - realSpeed) > abs(sumdis - GetDistance(gpsTrace[index - 1], gpsTrace[index]) - realSpeed))
+        index--;
+    return index;
+}
+
+iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + Veh_Width / 2 * cos(anglevalue + PI / 2),
+                       carFronty + Veh_Width / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + Veh_Width / 2 * cos(anglevalue - PI / 2),
+                        carFronty + Veh_Width / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>2.5 )
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                //		DecideGps00().lidarDistance = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}
+
+//1220
+iv::Point2D iv::decition::Compute00::getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue + PI / 2),
+                       carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + (Veh_Width-0.3) / 2 * cos(anglevalue - PI / 2),
+                        carFronty + (Veh_Width-0.3) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y<-1.0 )
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //+(ptx / abs(ptx))) / 2左右多出一半的车宽（1米）
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                //				int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                //				int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+                int dx = (ptx + gridwide*(double)centerx)/gridwide;
+
+                dx=grx-dx;//1227
+                int dy = (pty + gridwide*(double)centery)/gridwide;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    //					if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    if (lidarGridPtr[dx * (iv::gry) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                //		DecideGps00().lidarDistance = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistance = obsPoint.y;
+    return obsPoint;
+}
+
+
+iv::Point2D iv::decition::Compute00::getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr) {
+
+    iv::Point2D obsPoint(-1, -1);
+    vector<Point2D> gpsTraceLeft;
+    vector<Point2D> gpsTraceRight;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        //1127 fanwei xiuzheng
+        float buchang=0;
+        Point2D ptLeft(carFrontx + (Veh_Width+(buchang)*2) / 2 * cos(anglevalue + PI / 2),
+                       carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx +  (Veh_Width+(buchang)*2) / 2 * cos(anglevalue - PI / 2),
+                        carFronty +  (Veh_Width+(buchang)*2) / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceLeft.push_back(ptLeft);
+        gpsTraceRight.push_back(ptRight);
+
+    }
+
+
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        if (!isRemove && gpsTrace[j].y>2.5 && gpsTraceLeft[j].y>2.5 && gpsTraceRight[j].y>2.5)
+        {
+            int count = 0;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;  //*2左右多出一半的车宽（1米）
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            j++;
+
+            for (double length = 0; length <= Veh_Width; length += 0.4)
+            {
+                double ptx = gpsTraceLeft[j].x + (gpsTraceRight[j].x - gpsTraceLeft[j].x) / Veh_Width * length;
+                double pty = gpsTraceLeft[j].y + (gpsTraceRight[j].y - gpsTraceLeft[j].y) / Veh_Width * length;
+
+                int dx = (int)(ptx / gridwide * 2 + (ptx / abs(ptx))) / 2 + centerx;
+                int dy = (int)(pty / gridwide * 2 + (pty / abs(pty))) / 2 + centery;
+
+                if (dx >= 0 && dx <grx && dy >= 0 && dy < gry)
+                {
+                    if (lidarGridPtr[dx * (iv::gry + 1) + dy].ob != 0)
+                    {
+                        count++; obsPoint.x = ptx; obsPoint.y = pty;
+                    }
+                }
+            }
+
+            if (count >= 2)
+            {
+                obsPoint.x = gpsTrace[j].x;
+                obsPoint.y = gpsTrace[j].y;
+
+                isRemove = true;
+                DecideGps00().lidarDistanceAvoid = obsPoint.y;
+                return obsPoint;
+            }
+        }
+    }
+    //	DecideGps00().lidarDistanceAvoid = obsPoint.y;
+
+    return obsPoint;
+}
+
+
+
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace, std::vector<ObstacleBasic> esrRadars) {
+//	bool isRemove = false;
+//
+//	for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//	{
+//
+//		for (int i = 0; i < esrRadars.size(); i++)
+//			if ((esrRadars[i].nomal_y) != 0)
+//			{
+//				double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//				double yyy = esrRadars[i].nomal_y;
+//
+//				if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//				{
+//
+//					if (lastEsrID == (esrRadars[i]).esr_ID)
+//					{
+//						lastEsrCount++;
+//					}
+//					else
+//					{
+//						lastEsrCount = 0;
+//					}
+//
+//					if (lastEsrCount >= 3)
+//					{
+//						return i;
+//					}
+//
+//					lastEsrID = (esrRadars[i]).esr_ID;
+//				}
+//			}
+//	}
+//	return -1;
+//}
+
+
+
+
+int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum,int *esrPathpoint) {
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+//优化
+//                if(sqrt((xxx - gpsTrace[j].x)*(xxx - gpsTrace[j].x) + (yyy - gpsTrace[j].y)*(yyy - gpsTrace[j].y)) < (1.0*Veh_Width / 2.0+DecideGps00().xiuzhengCs)){
+//                    *esrPathpoint = j;
+//                    return i;
+//                }
+
+
+                if (abs(xxx - gpsTrace[j].x) <= (3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs) && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrID == i)
+                    {
+                        lastEsrCount++;
+                    }
+                    else
+                    {
+                        lastEsrCount = 0;
+                    }
+
+                    if(yyy>50 ){
+                        if (lastEsrCount >=200)
+                        {
+                            return i;
+                        }
+                    }
+                    else if (lastEsrCount >= 1)
+                    {
+                        return i;
+                    }
+
+                    lastEsrID = i;
+                }
+            }
+    }
+    return -1;
+}
+
+//int  iv::decition::Compute00::getEsrIndex(std::vector<Point2D> gpsTrace,int roadNum) {
+//    bool isRemove = false;
+
+//    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+//    {
+
+//        for (int i = 0; i < 64; i++)
+//            if ((examed_obs_radar[i].nomal_y) != 0 && (examed_obs_radar[i].valid))
+//            {
+//                double xxx = examed_obs_radar[i].nomal_x + Esr_Offset;
+//                double yyy = examed_obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+//                /*ODS("\nESR毫米波检测物体X距离:%f\n", xxx);
+//                ODS("\nESR毫米波检测物体Y距离:%f\n", yyy);*/
+
+
+//                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+//                {
+
+//                    if (lastEsrID == i)
+//                    {
+//                        lastEsrCount++;
+//                    }
+//                    else
+//                    {
+//                        lastEsrCount = 0;
+//                    }
+
+//                    if(yyy>50 ){
+//                        if (lastEsrCount >=200)
+//                        {
+//                            return i;
+//                        }
+//                    }
+//                    else if (lastEsrCount >= 3)
+//                    {
+//                        return i;
+//                    }
+
+//                    lastEsrID = i;
+//                }
+//            }
+//    }
+//    return -1;
+//}
+
+
+
+int  iv::decition::Compute00::getEsrIndexAvoid(std::vector<Point2D> gpsTrace) {
+    bool isRemove = false;
+
+    for (int j = 1; j < gpsTrace.size() - 1 && !isRemove; j++)
+    {
+
+        for (int i = 0; i < 64; i++)
+            if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid))
+            {
+                double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+                double yyy = ServiceCarStatus.obs_radar[i].nomal_y;
+
+                if (abs(xxx - gpsTrace[j].x) <= 3.0*Veh_Width / 4.0 && abs(yyy - (gpsTrace[j].y)) <= 1)
+                {
+
+                    if (lastEsrIDAvoid == i)
+                    {
+                        lastEsrCountAvoid++;
+                    }
+                    else
+                    {
+                        lastEsrCountAvoid = 0;
+                    }
+
+                    if (lastEsrCountAvoid >= 6)
+                    {
+                        return i;
+                    }
+
+                    lastEsrIDAvoid = i;
+                }
+            }
+    }
+    return -1;
+}
+
+
+
+
+
+
+
+//double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, std::vector<ObstacleBasic> esrRadars,double realSecSpeed) {
+//	double obsSpeed = 0 - realSecSpeed;
+//	double minDis = iv::MaxValue;
+//	for (int i = 0; i < esrRadars.size(); i++)
+//		if ((esrRadars[i].nomal_y) != 0)
+//		{
+//			double xxx = esrRadars[i].nomal_x + Esr_Offset;
+//			double yyy = esrRadars[i].nomal_y;
+//
+//			if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+//			{
+//				double tmpDis =sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+//				if (tmpDis < minDis)
+//				{
+//					minDis = tmpDis;
+//					obsSpeed = esrRadars[i].speed_y;
+//				}
+//			}
+//		}
+//
+//	return obsSpeed;
+//
+//
+//}
+
+
+
+
+double iv::decition::Compute00::getObsSpeed(Point2D obsPoint, double realSecSpeed) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+
+
+double iv::decition::Compute00::getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+
+    double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+
+    if (gpsTrace[0].v1 == 1)
+    {
+        KEang = 10; KEPos = 8;
+        if (realSpeed > 60) KEang = 5;
+    }
+    else if (gpsTrace[0].v1 == 2 || gpsTrace[0].v1 == 3)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 4 || gpsTrace[0].v1 == 5)
+    {
+        KEang = 14; KEPos = 10;
+    }
+    else if (gpsTrace[0].v1 == 7 && (gpsTrace[0].v2 == 23 || gpsTrace[0].v2 == 24))
+    {
+        KEang = 18; KEPos = 50; PreviewDistance = 3;
+    }
+    else if (gpsTrace[0].v1 == 7)
+    {
+        KEang = 20; KEPos = 50; PreviewDistance = 4;
+    }
+
+
+    double sumdis = 0;
+    int gpsIndex = 0;
+    std::vector<Point2D> farTrace;
+
+
+
+
+
+
+    for (int i = 1; i < gpsTrace.size() - 1; i++)
+    {
+        sumdis += GetDistance(gpsTrace[i - 1], gpsTrace[i]);
+        if (sumdis > PreviewDistance)
+        {
+            gpsIndex = i;
+            break;
+        }
+    }
+
+    if ((DecideGps00().readyParkMode) && (gpsIndex + 10>DecideGps00().gpsLineParkIndex))
+    {
+        gpsIndex = DecideGps00().gpsLineParkIndex;
+    }
+
+
+
+    EPos = gpsTrace[gpsIndex].x + avoidX;
+
+    for (unsigned int i = max(0, gpsIndex - 3); i < min((size_t)(gpsIndex + 3), gpsTrace.size()); i++) {
+        farTrace.push_back(gpsTrace[gpsIndex]);
+    }
+    if (farTrace.size() == 0) {
+        EAng = 0;
+    }
+    else {
+        EAng = getAvoidAveDef(farTrace, avoidX);
+    }
+
+    ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+
+    return ang;
+}
+
+
+std::vector<iv::GPSData>   iv::decition::Compute00::getBesideGpsMapLine(iv::GPS_INS now_gps_ins, vector<iv::GPSData>gpsMapLine, float avoidX) {
+
+    vector<vector<iv::GPSData>> maps;
+    vector<iv::GPSData> gpsMapLineBeside;
+    int sizeN = gpsMapLine.size();
+    for (int i = 1; i < sizeN; i++)
+    {
+        iv::GPSData gpsData(new GPS_INS);
+        double xx = gpsMapLine[i]->gps_x - now_gps_ins.gps_x;
+        double yy = gpsMapLine[i]->gps_y - now_gps_ins.gps_y;
+        double lng = ServiceCarStatus.location->ins_heading_angle;
+
+
+        double x0 = xx * cos(lng * PI / 180) - yy * sin(lng * PI / 180);
+        double y0 = xx * sin(lng * PI / 180) + yy * cos(lng * PI / 180);
+        double k1 = sin((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+        double k2 = cos((90 + (gpsMapLine[i]->ins_heading_angle - lng)) * PI / 180);
+
+        //	memcpy(&gpsData, &gpsMapLine[i], sizeof(gpsData));
+
+        gpsData->speed_mode = gpsMapLine[i]->speed_mode;
+        gpsData->gps_x = x0 + k1 * avoidX;
+        gpsData->gps_y = y0 + k2 * avoidX;
+        gpsMapLineBeside.push_back(gpsData);
+
+    }
+    return gpsMapLineBeside;
+
+}
+
+
+
+//double iv::decition::Compute00::getDecideAngleByLane(double realSpeed) {
+
+//    double ang = 0;
+//    double EPos = 0, EAng = 0;
+
+// //   double KEang = 14, KEpos = 10, DEang = 0, DEpos = 0;
+//       double KEang = 5, KEPos = 30, DEang = 0, DEPos = 0;
+
+// //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+//    double PreviewDistance;//预瞄距离
+//    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+////    if (realSpeed > 40)	KEang = 10; KEpos = 8;
+////        if (realSpeed > 50) KEang = 5;
+
+
+
+
+
+//double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+//double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+//double a = ServiceCarStatus.Lane.curvature;
+//double b = ServiceCarStatus.Lane.heading;
+//double c = (c1+c2)*0.5;
+//double x= PreviewDistance;
+//double y;
+
+
+//y=a*x*x+b*x+c;
+
+//   // EPos = y;
+//EPos=c;
+
+
+//  //  EAng=atan(2*a*x+b) / PI * 180;
+//    EAng=ServiceCarStatus.Lane.yaw;
+//        ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP);
+
+//        lastEA = EAng;
+//        lastEP = EPos;
+
+
+//           std::cout << "\nEPos:%f\n" << EPos << std::endl;
+//            std::cout << "\nEAng:%f\n" << EAng << std::endl;
+
+
+//        if (ang > angleLimit) {
+//            ang = angleLimit;
+//        }
+//        else if (ang < -angleLimit) {
+//            ang = -angleLimit;
+//        }
+//        if (lastAng != iv::MaxValue) {
+//            ang = 0.2 * lastAng + 0.8 * ang;
+//            //ODS("lastAng：%d\n", lastAng);
+//        }
+//        lastAng = ang;
+//        return ang;
+//    }
+
+
+
+double IEPos = 0, IEang = 0;
+
+
+double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
+
+    double ang = 0;
+    double EPos = 0, EAng = 0;
+    double Curve=0;
+    double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0;
+    double KCurve=120;
+    double KIEPos = 0, KIEang = 0;
+    //   double PreviewDistance = max(6.0, realSpeed / 3.6 * 1.8);//预瞄距离
+
+    double PreviewDistance;//预瞄距离
+
+    int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
+    int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
+    int conf =min(confL,confR);
+
+    realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(6.0, realSpeed *0.5);
+
+
+
+
+
+    if (realSpeed > 40)	KEang = 10; KEPos = 8;
+    if (realSpeed > 50) KEang = 5;
+
+    KEPos = 20;
+    KEang = 200;
+    //KEang = 15;
+
+    double c1 = ServiceCarStatus.aftermarketLane.dist_to_lane_l;
+    double c2 = ServiceCarStatus.aftermarketLane.dist_to_lane_r;
+    double a = ServiceCarStatus.Lane.curvature;
+    double b = ServiceCarStatus.Lane.heading;
+    double c = (c1+c2)*0.5;
+    double yaw= ServiceCarStatus.Lane.yaw;
+
+    double x= PreviewDistance;
+    double y;
+
+
+    y=c-(a*x*x+b*x);
+    double difa=0-(atan(2*a*x+b) / PI * 180);
+    Curve=0-a;
+
+    //EAng=difa;
+    //EPos=y;
+    EAng= 0-b;
+    EPos = c;
+    DEang = 10;
+    DEPos = 20;
+    //DEang = 20;
+    //DEPos = 10;
+
+    IEang = EAng+0.7*IEang;
+    IEPos = EPos+0.7*IEPos;
+    KIEang = 0;
+    //KIEang = 0.5;
+    KIEPos =2;
+
+
+
+    if(abs(confL)>=2&&abs(confR)>=2){
+        //ang = KEang * EAng + KEPos * EPos + DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+        ang = KEang * EAng + KEPos * EPos +KCurve*Curve+ DEang * (EAng - lastEA) + DEPos * (EPos - lastEP)+ KIEang * IEang + KIEPos * IEPos;
+    }else{
+        ang=lastAng;
+    }
+    //if(lastAng!=0&&abs(ang-lastAng)>20)ang=lastAng;
+
+    lastEA = EAng;
+    lastEP = EPos;
+
+    if (ang > angleLimit) {
+        ang = angleLimit;
+    }
+    else if (ang < -angleLimit) {
+        ang = -angleLimit;
+    }
+    if (lastAng != iv::MaxValue) {
+        ang = 0.2 * lastAng + 0.8 * ang;
+        //ODS("lastAng：%d\n", lastAng);
+    }
+    lastAng = ang;
+    return ang;
+}
+
+
+
+double iv::decition::Compute00::bocheCompute(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 x_1 = pt.x;
+    double y_1 = pt.y;
+    double angle_1 = getQieXianAngle(nowGps,aimGps);
+    double x_2 = 0.0, y_2 = 0.0;
+    double steering_angle;
+    double l = 2.950;
+    double r =6;
+    double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double g_1 = tan(angle_1);
+    double car_pos[3] = { x_1,y_1,g_1 };
+    double parking_pos[2] = { x_2,y_2 };
+    double g_3;
+    double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+
+    //g_3 = 0 - 0.5775;
+    g_3 = pingfenxian_xielv(x_1, y_1, x_2, y_2, angle_1);
+    //交点
+    x_3 = 0.0;//(y_1 - y_2 + g_2*x_2 - g_1*x_1) / (g_2 - g_1);
+    y_3 = y_1 - g_1 * x_1;
+    //圆心1
+    x_o_1 = r;
+    y_o_1 = g_3 * r + y_3;
+    //圆形1的切点1
+    x_t_1 = 0.0;
+    y_t_1 = g_3 * r + y_3;
+    //圆形1的切点2
+    if (g_1 == 0)
+    {
+        x_t_2 = r;
+        y_t_2 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_2 = (y_1 + g_1 * x_o_1 + y_o_1 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_2 = (y_t_2 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    //圆心2
+    x_o_2 = 0 - r;
+    y_o_2 = y_3 - g_3 * r;
+    //圆形2的切点1
+    x_t_3 = 0;
+    y_t_3 = y_3 - g_3 * r;
+    //圆形2的切点2
+    if (g_1 == 0)
+    {
+        x_t_4 = 0 - r;
+        y_t_4 = y_1 - g_1 * x_1;
+    }
+    else
+    {
+        y_t_4 = (y_1 + g_1 * x_o_2 + y_o_2 * g_1*g_1 - g_1 * x_1) / (1 + g_1 * g_1);
+        x_t_4 = (y_t_4 + g_1 * x_1 - y_1) / g_1;
+
+    }
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            x_t_n = x_t_1;
+            y_t_n = y_t_1;
+            x_t_f = x_t_2;
+            y_t_f = y_t_2;
+        }
+        else
+        {
+            x_t_n = x_t_2;
+            y_t_n = y_t_2;
+            x_t_f = x_t_1;
+            y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            x_t_n = x_t_3;
+            y_t_n = y_t_3;
+            x_t_f = x_t_4;
+            y_t_f = y_t_4;
+        }
+        else
+        {
+            x_t_n = x_t_4;
+            y_t_n = y_t_4;
+            x_t_f = x_t_3;
+            y_t_f = y_t_3;
+        }
+
+
+
+
+    }
+    steering_angle = atan2(l, r);
+
+    if (x_t_n < 0)
+    {
+        steering_angle = 0 - steering_angle;
+    }
+
+    nearTpoint=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    farTpoint = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    bocheAngle = steering_angle*180/PI;
+
+    cout << "近切点：x_t_n=" << x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    cout << "航向角：" << steering_angle << endl;
+
+
+    //    if (x_1 < 0 && y_1 > 0 && x_1 < x_t_n &&y_t_f > 0.1) {
+    //        return 1;
+    //    }
+    Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
+    double disA = GetDistance(aimGps,nowGps);
+    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
+        return 1;
+    }
+
+    return 0;
+
+}
+
+
+
+//返回垂直平分线的斜率
+double iv::decition::Compute00::pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1) {
+    double angl, x_3, angle_3;
+    if (tan(angle_1 == 0))
+    {
+        if ((x_1 - x_2) > 0 && ((y_1 - y_2) > 0))
+        {
+            angle_3 = 0 - 1;
+        }
+        else
+        {
+            angle_3 = 1;
+        }
+    }
+    else
+    {
+        x_3 = (tan(angle_1)*x_1 - y_1) / tan(angle_1);//车所在直线与x轴交点
+        angl = tan(angle_1);//车所在直线的斜率
+        if ((x_1 - x_2)>0 && ((y_1 - y_2)>0))//第一象限
+        {
+            if ((angl *x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(PI*0.5 + (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+
+
+            }
+
+        }
+        else//第二象限
+        {
+            if ((angl*x_3)<0)//车斜率与车直线的x轴交点异号
+            {
+                if (angl < 0)
+                {
+                    angle_3 = tan(PI*0.5 - (PI*0.5 + atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+                else
+                {
+                    angle_3 = tan(atan(fabs(angl)) + (PI*0.5 - atan(fabs(angl))) *0.5);//垂直平分线斜率
+                }
+
+            }
+        }
+    }
+
+    return angle_3;
+
+}
+
+
+
+double iv::decition::Compute00::getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps) {
+    double heading = nowGps.ins_heading_angle *PI/180;
+    double x1 = nowGps.gps_x;
+    double y1 = nowGps.gps_y;
+    if (heading<=PI*0.5)
+    {
+        heading = 0.5*PI - heading;
+    }
+    else if (heading>PI*0.5 && heading<=PI*1.5) {
+        heading = 1.5*PI - heading;
+    }
+    else if (heading>PI*1.5) {
+        heading = 2.5*PI - heading;
+    }
+    double k1 = tan(heading);
+    double x = x1+10;
+    double y = k1 * x + y1 - (k1 * x1);
+    Point2D pt1 = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    Point2D pt2 = Coordinate_Transfer(x, y, aimGps);
+    double xielv = (pt1.y - pt2.y) / (pt1.x - pt2.x);
+    double angle = atan(abs(xielv));
+    if (xielv<0)
+    {
+        angle = PI - angle;
+    }
+    return angle;
+
+}
+
+
+/*
+  chuizhicheweiboche
+  */
+
+
+int iv::decition::Compute00::bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps)
+{
+
+
+
+    double l=2.95;//轴距
+    double x_0 = 0, y_0 = 0.5;
+    double x_1, y_1;//车起点坐标
+    double ange1;//车航向角弧度
+    double x_2, y_2;//另一条与车直线在angle2和R_M 固定情况下过坐标点，第二个近切点
+    double real_rad;;//另一条直线的航向角弧度
+    double angle_3;//垂直平分线弧度
+    double x_3, y_3;//垂直平分线交点
+    double x_4, y_4;//另一条直线的远切点坐标，第二个远切点，已知
+    double x_o_1, y_o_1;//圆形1坐标
+    double x_o_2, y_o_2;//圆形2坐标
+    double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
+    double min_rad;
+    double R_M; //后轴中点的转弯半径
+    double steering_angle;
+
+
+
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+    x_1=pt.x;
+    y_1=pt.y;
+    ange1=getQieXianAngle(nowGps,aimGps);
+
+    min_rad_zhuanxiang(&R_M , &min_rad);
+    qiedian_n(x_1,y_1,R_M,min_rad,&x_2 , &y_2, &real_rad);//计算另一条与车直线在angle2和R_M 固定情况下近切点：x_2, y_2
+    liangzhixian_jiaodian( x_1, y_1,  x_2, y_2,ange1,real_rad,&x_3 , &y_3);
+    chuizhipingfenxian_xielv( x_1, y_1, ange1, real_rad, min_rad,&angle_3);
+    yuanxin( x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_o_1,&y_o_1,&x_o_2,&y_o_2);
+    yuanxin_qiedian( ange1, x_o_1, y_o_1,  x_o_2, y_o_2,
+                     x_1, y_1, x_2, y_2, x_3, y_3, real_rad, angle_3, R_M,&x_t_n,&y_t_n,&x_t_f, &y_t_f);
+    steering_angle = atan2(l, R_M);
+    x_4 = 0.5;
+    y_4 = 0;
+    //for (int i = 0; i < 4; i++)
+    //{
+    //for (int j = 0; j < 2; j++)
+    //{
+    //	cout << t[i][j] << endl;
+    //}
+    //}
+    //cout << "min_rad:" << min_rad<< endl;
+    //cout << "jiaodian：x=" << x_3 << endl;
+    //cout << "jiaodian：y=" << y_3 << endl;
+    // cout << "R-M：" << R_M << endl;
+    cout << "x_0:" << x_0 << endl;
+    cout << "y_0:" << y_0 << endl;
+    cout << "x_2:" << x_2 << endl;
+    cout << "y_2:" << y_2 << endl;
+    cout << "近切点：x_t_n="<< x_t_n << endl;
+    cout << "近切点：y_t_n=" << y_t_n << endl;
+    cout << "远切点：x_t_f=" << x_t_f << endl;
+    cout << "远切点：y_t_f=" << y_t_f << endl;
+    //cout << "航向角：" << steering_angle << endl;
+    //cout << "圆心1横坐标=" << x_o_1 << endl;
+    //cout << "圆心1纵坐标=" << y_o_1 << endl;
+    //cout << "圆心2横坐标=" << x_o_2 << endl;
+    //cout << "圆心2纵坐标=" << y_o_2 << endl;
+    //cout << "平分线弧度=" << angle_3 << endl;
+    //cout << " min_rad=" << min_rad << endl;
+    //cout << " real_rad=" << real_rad << endl;
+    //   system("PAUSE");
+
+
+
+    dTpoint0=Coordinate_UnTransfer(x_t_n, y_t_n, aimGps);
+    dTpoint1 = Coordinate_UnTransfer(x_t_f, y_t_f, aimGps);
+    dTpoint2 = Coordinate_UnTransfer(x_2, y_2, aimGps);
+    dTpoint3 = Coordinate_UnTransfer(x_0, y_0, aimGps);
+    dBocheAngle = steering_angle*180/PI;
+
+    double disA = GetDistance(aimGps,nowGps);
+
+    if(pt.y>y_t_n && x_t_f<x_2 && y_t_f>y_2&&disA<40){
+        return 1;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad) {
+    double L_c = 4.749;//车长
+    double rad_1;
+    double rad_2;
+    double L_k = 1.931;//车宽
+    double L = 2.95;//轴距
+    double L_f =1.2 ;//前悬
+    double L_r =0.7 ;//后悬
+    double R_min =6.5 ;//最小转弯半径
+    *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
+    //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
+    //rad_2 = atan2(L + L_f, R_M + L_k*0.5);
+    *min_rad = 45 * PI / 180;// rad_1 - rad_2;
+    return 0;
+}
+
+
+double iv::decition::Compute00::qiedian_n(double x_1, double y_1, double R_M,double min_rad, double *x_2, double *y_2, double *real_rad ) {
+
+    if (x_1 > 0 && y_1 > 0)
+    {
+        *real_rad = PI*0.5 - min_rad;
+        *x_2 = R_M - R_M*cos(min_rad);
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    else
+    {
+        *real_rad = PI*0.5 + min_rad;
+        *x_2 = R_M*cos(min_rad) - R_M;
+        *y_2 = R_M*sin(min_rad) + 0.5;
+    }
+    return 0;
+
+}
+
+
+double iv::decition::Compute00::liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,double ange1,double real_rad,double *x_3,double *y_3) {
+    double b1, b2;
+    double k1, k2;
+    if (ange1!=(PI*0.5))
+    {
+        k1 = tan(ange1);
+        b1 = y_1 - k1*x_1;
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = (b2 - b1) / (k1 - k2);
+        *y_3 = k2*(*x_3) + b2;
+    }
+    else
+    {
+        k2 = tan(real_rad);
+        b2 = y_2 - k2*x_2;
+        *x_3 = x_1;
+        *y_3 = k2*(*x_3) + b2;
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,double min_rad,double *angle_3) {
+    double k1, k2;
+    double  angle_j;
+    k2 = tan(real_rad);
+    if (ange1 != (PI*0.5))
+    {
+        k1 = tan(ange1);
+        angle_j = atan(fabs((k2 - k1) / (1 + k2*k1)));//两直线夹角
+
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    else
+    {
+        angle_j = min_rad;//两直线夹角
+        if (x_1 > 0 && y_1 > 0)
+        {
+            *angle_3 = angle_j*0.5 - min_rad + PI;
+        }
+        else
+        {
+            *angle_3 = min_rad - angle_j*0.5;
+        }
+    }
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                        double *x_o_1,double *y_o_1,double *x_o_2,double *y_o_2) {
+    double b2, b3, k2, k3;
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(angle_3)*x_3;
+    k2 = tan(real_rad);
+    k3 = tan(angle_3);
+    *x_o_1 = (sqrt(k2*k2 + 1)*R_M + b3 - b2) / (k2 - k3);
+    *y_o_1 = k3*(*x_o_1) + b3;
+
+    *x_o_2 = (b3 - b2 - (sqrt(k2*k2 + 1)*R_M)) / (k2 - k3);
+    *y_o_2 = k3*(*x_o_2) + b3;
+    return 0;
+}
+
+
+double iv::decition::Compute00::yuanxin_qiedian(double ange1,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                                double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,double real_rad,double angle_3,double R_M,
+                                                double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f)
+{
+    double x_o, y_o;
+    double b2, b3, k1, k2, k3;
+    //double car_pos[3] = { x_1,y_1,k1 };
+    double parking_pos[2] = { x_2,y_2 };
+    //double t[4][2];
+    double p[4];
+    double  s1, s2; //切点与车起始位置的距离
+    double  min;
+    int  min_i;
+    double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
+    double x_t_3, y_t_3, x_t_4, y_t_4;//圆形2的切点
+    double t[4][2];
+    k1 = tan(ange1);
+    b2 = y_2 - tan(real_rad)*x_2;
+    b3 = y_3 - tan(real_rad)*x_3;
+    k2 = tan(real_rad);//另一条直线斜率
+    k3 = tan(angle_3);//垂直平分线斜率
+    //圆心1和2切点*********************************************
+    if (x_1 > 0 && y_1 > 0)//第一象限
+    {
+        if (ange1 == (PI*0.5))
+        {
+            x_t_1 = x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+    }
+    else
+    {
+        if (ange1 == 0)
+        {
+            x_t_1 = 0 - x_1;
+            y_t_1 = y_o_1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            x_t_3 = 0 - x_1;
+            y_t_3 = y_o_2;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+        }
+        else
+        {
+            y_t_1 = (y_1 + k1 *x_o_1 + y_o_1*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_1 = (y_t_1 + k1*x_1 - y_1) / k1;
+            y_t_2 = (y_2 + k2 *x_o_1 + y_o_1*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_2 = (y_t_2 + k2*x_2 - y_2) / k2;
+
+            y_t_3 = (y_1 + k1 *x_o_2 + y_o_2*k1*k1 - k1*x_1) / (1 + k1*k1);
+            x_t_3 = (y_t_3 + k1*x_1 - y_1) / k1;
+            y_t_4 = (y_2 + k2 *x_o_2 + y_o_2*k2*k2 - k2*x_2) / (1 + k2*k2);
+            x_t_4 = (y_t_4 + k2*x_2 - y_2) / k2;
+
+        }
+
+    }
+
+    //圆心1和2切点*********************************************
+
+    t[0][0] = x_t_1;
+    t[0][1] = y_t_1;
+    t[1][0] = x_t_2;
+    t[1][1] = y_t_2;
+    t[2][0] = x_t_3;
+    t[2][1] = y_t_3;
+    t[3][0] = x_t_4;
+    t[3][1] = y_t_4;
+    for (int i = 0; i < 4; i++)
+    {
+
+        p[i] = (t[i][0] - parking_pos[0])*(t[i][0] - parking_pos[0]) + (t[i][1] - parking_pos[1])*(t[i][1] - parking_pos[1]);
+
+    }
+    min = p[0];
+    min_i = 0;
+    for (int i = 1; i < 4; i++)
+    {
+
+        if (p[i] < min)
+        {
+            min = p[i]; min_i = i;
+        }
+    }
+    if (min_i < 2)
+    {
+        x_o = x_o_1;
+        y_o = y_o_1;
+        s1 = (x_t_1 - x_1)*(x_t_1 - x_1) + (y_t_1 - y_1)*(y_t_1 - y_1);
+        s2 = (x_t_2 - x_1)*(x_t_2 - x_1) + (y_t_2 - y_1)*(y_t_2 - y_1);
+        if (s1 < s2)
+        {
+            *x_t_n = x_t_1;
+            *y_t_n = y_t_1;
+            *x_t_f = x_t_2;
+            *y_t_f = y_t_2;
+        }
+        else
+        {
+            *x_t_n = x_t_2;
+            *y_t_n = y_t_2;
+            *x_t_f = x_t_1;
+            *y_t_f = y_t_1;
+
+        }
+    }
+    else
+    {
+        x_o = x_o_2;
+        y_o = y_o_2;
+        s1 = (x_t_3 - x_1)*(x_t_3 - x_1) + (y_t_3 - y_1)*(y_t_3 - y_1);
+        s2 = (x_t_4 - x_1)*(x_t_4 - x_1) + (y_t_4 - y_1)*(y_t_4 - y_1);
+
+        if (s1 < s2)
+        {
+
+            *x_t_n = x_t_3;
+            *y_t_n = y_t_3;
+            *x_t_f = x_t_4;
+            *y_t_f = y_t_4;
+        }
+        else
+        {
+            *x_t_n = x_t_4;
+            *y_t_n = y_t_4;
+            *x_t_f = x_t_3;
+            *y_t_f = y_t_3;
+        }
+
+
+
+    }
+
+    return 0;
+
+}
+
+
+int iv::decition::Compute00::getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap)
+{
+    int index = -1;
+    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
+    int endIndex = gpsMap.size() - 1;
+    float minDis=20;
+
+    for (int j = startIndex; j < endIndex; j++)
+    {
+        int i = (j + gpsMap.size()) % gpsMap.size();
+        double tmpdis = GetDistance(rp, (*gpsMap[i]));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis=tmpdis;
+        }
+    }
+    return index;
+}
+
+
+double iv::decition::Compute00::getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const vector<Point2D>& gpsTrace, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {
+    double obsSpeed = 0 - realSecSpeed;
+    double minDis = iv::MaxValue;
+    FrenetPoint esr_obs_F_point;
+    for (int i = 0; i < 64; i++)
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && ServiceCarStatus.obs_radar[i].valid)
+        {
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y + Esr_Y_Offset;
+
+            if (abs(xxx - obsPoint.x) < 4 && abs(yyy - obsPoint.y) < 2)
+            {
+                double tmpDis = sqrt((xxx - obsPoint.x) * (xxx - obsPoint.x) + (yyy - obsPoint.y) * (yyy - obsPoint.y));
+                if (tmpDis < minDis)
+                {
+                    minDis = tmpDis;
+//                    esr_obs_F_point = iv::decition::FrenetPlanner::XY2Frenet(xxx, yyy, gpsTrace);
+                    esr_obs_F_point = iv::decition::FrenetPlanner::getFrenetfromXY(xxx, yyy, gpsTrace,gpsMap,pathpoint,nowGps);
+//                    obsSpeed = ServiceCarStatus.obs_radar[i].speed_y;
+                    double speedx=ServiceCarStatus.obs_radar[i].speed_x;  //障碍物相对于车辆x轴的速度
+                    double speedy=ServiceCarStatus.obs_radar[i].speed_y;  //障碍物相对于车辆y轴的速度
+                    double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
+                    //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+                    //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+                    double Etheta = esr_obs_F_point.tangent_Ang - atan2(speedy,speedx);
+
+                    obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+                }
+            }
+        }
+
+    return obsSpeed;
+}
+
+
+int iv::decition::Compute00::getEsrIndexByFrenet(const std::vector<Point2D>& gpsTrace, FrenetPoint &esrObsPoint, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps){
+    double minDistance = numeric_limits<double>::max();
+    int minDis_index=-1;
+
+    for(int i=0; i<64; ++i){
+        if ((ServiceCarStatus.obs_radar[i].nomal_y) != 0 && (ServiceCarStatus.obs_radar[i].valid)){
+            //毫米波在车头，故要加上毫米波与惯导的相对距离。(xxx,yyy)才是障碍物在 车辆坐标系下的坐标。
+            double xxx = ServiceCarStatus.obs_radar[i].nomal_x + Esr_Offset;
+            double yyy = ServiceCarStatus.obs_radar[i].nomal_y+ Esr_Y_Offset;
+
+            //将毫米波障碍物位置转换到frenet坐标系下
+//            esrObsPoint = iv::decition::FrenetPlanner::XY2Frenet(xxx,yyy,gpsTrace);
+            esrObsPoint = iv::decition::FrenetPlanner::getFrenetfromXY(xxx,yyy,gpsTrace,gpsMap,pathpoint,nowGps);
+
+            //如果障碍物与道路的横向距离d<=3.0*Veh_Width / 4.0，则认为道路上有障碍物。
+            //s则可理解为障碍物距离。为障碍物与车辆沿着道路方向的距离，而不是空间上的x或y坐标或者直线距离。
+            //minDistance、minDis_index用来统计最近的障碍物信息。
+            if(abs(esrObsPoint.d)<=(3.0*Veh_Width / 4.0+DecideGps00().xiuzhengCs)){
+                if(esrObsPoint.s<minDistance){
+                    minDistance = esrObsPoint.s;
+                    minDis_index = i;
+                }
+            }
+        }
+    }
+    return minDis_index;
+}
+
+
+
+
+std::vector<std::vector<iv::GPSData>> gmapsL;
+std::vector<std::vector<iv::GPSData>> gmapsR;

src/decition/decition_brain_sf_add_frenet_plan/decition/compute_00.h

@@ -0,0 +1,96 @@
+#pragma once
+#ifndef _IV_DECITION_COMPUTE_00_
+#define _IV_DECITION_COMPUTE_00_
+
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+#include <decition/decide_gps_00.h>
+
+namespace iv {
+    namespace decition {
+    //根据传感器传输来的信息作出决策
+        class Compute00 {
+        public:
+            Compute00();
+            ~Compute00();
+
+            static   double maxAngle;
+            static	 double angleLimit;  //角度限制
+            static	 double lastEA;      //上一次角度误差
+            static	 double lastEP;      //上一次位置误差
+            static	 double decision_Angle;  //决策角度
+            static	 double lastAng;         //上次角度
+            static   int lastEsrID;          //上次毫米波障碍物ID
+            static   int  lastEsrCount;      //毫米波障碍物累计次数
+
+            static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
+            static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
+
+            static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
+            static double bocheAngle,dBocheAngle;
+
+            static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
+            static	int getFirstNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
+
+            static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
+            static double getAveDef(std::vector<Point2D> farTrace);
+            static int getSpeedPointIndex(std::vector<Point2D> gpsTrace, double realSpeed);
+            static Point2D getLidarObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static Point2D getLidarRearObsPoint(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+
+
+            static Point2D getLidarObsPointAvoid(std::vector<Point2D> gpsTrace, iv::LidarGridPtr lidarGridPtr);
+            static int getEsrIndex(std::vector<Point2D> gpsTrace , int roadNum, int *esrPathpoint);
+            static int getEsrIndexAvoid(std::vector<Point2D> gpsTrace);
+
+            static double getObsSpeed(Point2D obsPoint, double realSecSpeed);
+
+            static double getAvoidAveDef(std::vector<Point2D> farTrace, double avoidX);
+            static double getDecideAvoidAngle(std::vector<Point2D>  gpsTrace, double realSpeed, float avoidX);
+
+            static std::vector<GPSData>  getBesideGpsMapLine(iv::GPS_INS now_gps_ins, std::vector<iv::GPSData>gpsMapLine, float avoidX);
+
+            static double getDecideAngleByLane(double realSpeed);
+
+            static double getDecideAngleByLanePID(double realSpeed);
+
+            static double bocheCompute(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);
+
+            static int bocheDirectCompute(GPS_INS nowGps, GPS_INS aimGps);
+
+            static double min_rad_zhuanxiang(double *R_M, double *min_rad);
+            static double qiedian_n(double x_1,double y_1,double R_M,double min_rad,
+                                    double *x_2, double *y_2, double *real_rad);
+            static double liangzhixian_jiaodian(double x_1,double y_1,double x_2,double y_2,
+                                                double ange1,double real_rad,double *x_3, double *y_3);
+            static double chuizhipingfenxian_xielv(double x_1,double y_1,double ange1,double real_rad,
+                                                   double min_rad,double *angle_3);
+            static double yuanxin(double x_2,double y_2,double x_3,double y_3,double real_rad,
+                                  double angle_3,double R_M,double *x_o_1, double *y_o_1, double *x_o_2, double *y_0_2);
+            static double yuanxin_qiedian(double angel,double x_o_1,double y_o_1, double x_o_2,double y_o_2,
+                                          double x_1,double y_1,double x_2,double y_2,double x_3,double y_3,
+                                          double real_rad,double angle_3,double R_M,
+                                          double *x_t_n, double *y_t_n, double *x_t_f, double *y_t_f);
+
+            static int getNoAngleNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap);
+    		static int getEsrIndexByFrenet(const std::vector<Point2D>& gpsTrace,FrenetPoint &esrObsPoint, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+    		double getObsSpeedByFrenet(Point2D obsPoint, double realSecSpeed,const std::vector<Point2D>& gpsTrace, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+        private:
+
+        };
+    }
+}
+extern std::vector<std::vector<iv::GPSData>> gmapsL;
+extern std::vector<std::vector<iv::GPSData>> gmapsR;
+
+extern std::vector<int> lastEsrIdVector;
+extern std::vector<int> lastEsrCountVector;
+
+#endif // !_IV_DECITION_COMPUTE_00_
+

src/decition/decition_brain_sf_add_frenet_plan/decition/decide_from_gps.cpp

@@ -0,0 +1,605 @@
+#include <decition/decition_maker.h>
+#include <decition/gps_distance.h>
+#include <decition/decition_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+using namespace std;
+
+
+iv::decition::DecitionMaker::DecitionMaker() {
+
+}
+iv::decition::DecitionMaker::~DecitionMaker() {
+
+}
+
+
+
+
+
+
+/* void iv::decition::DecitionMaker::decideFromGPS(Decition & gps_decition, iv::GPSData gps_data, const std::vector<iv::GPSData> navigation_data)
+{
+	//可直接获取车速  方法咨询张庆余
+
+	//样例
+	//输入数据类型
+	gps_data->gps_x;
+	gps_data->gps_y;
+	gps_data->gps_z;
+	gps_data->ins_heading_angle;	//航向角
+	gps_data->ins_pitch_angle;		//俯仰角
+	gps_data->ins_roll_angle;		//横滚角
+
+									//导航数据
+	navigation_data[0]->gps_x;
+	navigation_data[1]->gps_y;
+	for (std::vector<iv::GPSData>::const_iterator it = navigation_data.cbegin(); it != navigation_data.cend(); it++) {
+		//std::cout << (*it)->gps_x << std::endl;
+	}
+
+	//输出
+	gps_decition->speed = 30;
+	gps_decition->wheel_angle = 10;
+
+}*/
+
+
+void iv::decition::DecitionMaker::adjustOriginPoint(iv::GPS_INS nowGPSIns) {
+	const double PI = 3.1415926535898;
+	if (nowGPSIns.ins_heading_angle >= 0 && nowGPSIns.ins_heading_angle <= 90) {
+		origin_x = nowGPSIns.gps_x + vehLenthAdj*sin(nowGPSIns.ins_heading_angle*PI / 180);
+		origin_y = nowGPSIns.gps_y + vehLenthAdj*cos(nowGPSIns.ins_heading_angle*PI / 180);
+	}
+	else if (nowGPSIns.ins_heading_angle >= 90 && nowGPSIns.ins_heading_angle <= 180)
+	{
+		origin_x = nowGPSIns.gps_x + vehLenthAdj*sin((180 - nowGPSIns.ins_heading_angle)*PI / 180);
+		origin_y = nowGPSIns.gps_y - vehLenthAdj*cos((180 - nowGPSIns.ins_heading_angle)*PI / 180);
+	}
+	else  if (nowGPSIns.ins_heading_angle >= 180 && nowGPSIns.ins_heading_angle <= 270)
+	{
+		origin_x = nowGPSIns.gps_x - vehLenthAdj*sin((nowGPSIns.ins_heading_angle - 180)*PI / 180);
+		origin_y = nowGPSIns.gps_y - vehLenthAdj*cos((nowGPSIns.ins_heading_angle - 180)*PI / 180);
+	}
+	else if (nowGPSIns.ins_heading_angle >= 270 && nowGPSIns.ins_heading_angle <= 360)
+	{
+		origin_x = nowGPSIns.gps_x - vehLenthAdj*sin((360 - nowGPSIns.ins_heading_angle)*PI / 180);
+		origin_y = nowGPSIns.gps_y + vehLenthAdj*cos((360 - nowGPSIns.ins_heading_angle)*PI / 180);
+	}
+
+}
+
+
+
+
+/*
+	开启自动驾驶模式时，先获取离汽车距离最近的路径点
+*/
+
+ void iv::decition::DecitionMaker::getStartPoint(iv::GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data) {
+	 double minDistance,distance;
+	 for (int i = lastNearPointNum; i < navigation_data.size(); i++)
+	 {
+		 distance = DirectDistance(origin_x, origin_y,(navigation_data[i])->gps_x, (navigation_data[i])->gps_y);
+//		 distance = CalcDistance(gps_ins.gps_lat, gps_ins.gps_lng, (navigation_data[i])->gps_lat, (navigation_data[i])->gps_lng);
+		 if (i==0)
+		 {
+			 minDistance = distance;
+		 }
+		 else {
+			 if (distance<minDistance)
+			 {
+				 minDistance = distance;
+				 lastNearPointNum = i;
+                 std::cout << "lastnearestpoint: %d\n" << lastNearPointNum << std::endl;
+//				 ODS("minDistance: %d\n", minDistance);
+			 }
+		 }
+	 }
+
+}
+
+
+
+ /*
+     根据GPS坐标计算两点距离
+ */
+
+double iv::decition::DecitionMaker::getDistanceDeviation(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns) {
+	return	CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nowGPSIns).gps_lat, (nowGPSIns).gps_lng);
+}
+
+
+/*
+    根据GPS坐标计算汽车与目标点之间的角度
+*/
+
+double iv::decition::DecitionMaker::getCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns) {
+	const double PI = 3.1415926535898;
+	double hypo = CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nowGPSIns).gps_lat, (nowGPSIns).gps_lng);
+	double hor = CalcDistance((nearestGPSIns).gps_lat, (nearestGPSIns).gps_lng, (nearestGPSIns).gps_lat, (nowGPSIns).gps_lng);
+	double ang = asin(hor / hypo) / PI * 180;
+	double guideAng;
+	double crossAng; //顺时针是正直，逆时针是负值
+	if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat >= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng >= 0)) {
+		guideAng = ang;
+		//ang = 90 - ang;
+	}
+	else if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat >= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng <= 0))
+	{
+		guideAng = 360 - ang;
+		//ang = ang + 90;
+		
+	}
+	else if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat <= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng >= 0))
+	{
+		guideAng = 180 - ang;
+		//ang = ang - 90;
+	}
+	else  if (((nearestGPSIns).gps_lat - (nowGPSIns).gps_lat <= 0) && ((nearestGPSIns).gps_lng - (nowGPSIns).gps_lng <= 0))
+	{
+		guideAng = 180 + ang;
+		//ang = -90 - ang;
+	}
+
+	crossAng = guideAng - nowGPSIns.ins_heading_angle;
+	if (crossAng<-180)
+	{
+		crossAng = crossAng + 360;
+	}
+	else if (crossAng > 180) {
+		crossAng = crossAng - 360;
+	}
+	
+	
+	return crossAng;
+
+}
+
+/*
+	根据直角坐标计算汽车与目标点之间的角度
+*/
+
+double iv::decition::DecitionMaker::getDirectCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS objGPSIns) {
+
+	//double ang = DirectAngle(nowGPSIns.gps_x, nowGPSIns.gps_y,objGPSIns.gps_x,objGPSIns.gps_y);
+	double ang = DirectAngle(origin_x, origin_y, objGPSIns.gps_x, objGPSIns.gps_y);
+	double guideAng; //追随目标所需航向角
+	double crossAng; //顺时针是正直，逆时针是负值
+	if (((objGPSIns).gps_y - origin_y >= 0) && ((objGPSIns).gps_x - origin_x >= 0)) {
+		guideAng = 90 - ang;
+		//ang = 90 - ang;
+	}
+	else if (((objGPSIns).gps_y - origin_y >= 0) && ((objGPSIns).gps_x - origin_x <= 0))
+	{
+		guideAng = 270 - ang;
+		//ang = ang + 90;
+
+	}
+	else if (((objGPSIns).gps_y - origin_y <= 0) && ((objGPSIns).gps_x - origin_x >= 0))
+	{
+		guideAng = 90 - ang;
+		//ang = ang - 90;
+	}
+	else  if (((objGPSIns).gps_y - origin_y <= 0) && ((objGPSIns).gps_x - origin_x <= 0))
+	{
+		guideAng = 270 - ang;
+		//ang = -90 - ang;
+	}
+
+	crossAng = guideAng - nowGPSIns.ins_heading_angle;
+	if (crossAng<-180)
+	{
+		crossAng = crossAng + 360;
+	}
+	else if (crossAng > 180) {
+		crossAng = crossAng - 360;
+	}
+	return crossAng;
+}
+
+
+
+
+
+/*
+		根据GPS坐标搜索最近的车前路径点
+*/
+
+iv::GPS_INS iv::decition::DecitionMaker::getNearestGpsIns(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data) {
+	
+	GPS_INS lastNearPoint = *(navigation_data[lastNearPointNum]);
+
+	int count = navigation_data.size();
+
+	if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
+		for (int i = lastNearPointNum; i < count; i++) {
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_y>origin_y)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
+
+		for (int i = lastNearPointNum; i < count; i++)
+		{
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_x>origin_x)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
+
+		for (int i = lastNearPointNum; i < count; i++)
+		{
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_y<origin_y)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
+
+		for (int i = lastNearPointNum; i < count; i++)
+		{
+			GPS_INS nearGNS = *(navigation_data[i]);
+
+			if ((nearGNS).gps_x<origin_x)
+			{
+				lastNearPointNum = i;
+				return nearGNS;
+			}
+		}
+
+	}
+
+	return lastNearPoint;
+}
+
+
+/*
+  判断是否到达终点
+*/
+
+bool iv::decition::DecitionMaker::checkComplete(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data) {
+	if (lastNearPointNum < navigation_data.size() - 1) {
+		return false;
+	}
+	
+	GPS_INS lastNearPoint = *(navigation_data[lastNearPointNum]);
+
+	if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
+		if (origin_y>=lastNearPoint.gps_y)
+		{
+			return true;
+		}
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
+		if (origin_x >= lastNearPoint.gps_x)
+		{
+			return true;
+		}
+		
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
+
+		if (origin_y <= lastNearPoint.gps_y)
+		{
+			return true;
+		}
+
+	}
+	else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
+
+		if (origin_x <= lastNearPoint.gps_x)
+		{
+			return true;
+		}
+
+	}
+
+	return false;
+
+}
+
+
+
+
+
+
+/*
+	计算输出车速
+*/
+
+float iv::decition::DecitionMaker::getSpeedObj(GPS_INS nowGPSIns, GPS_INS objGPSIns) {
+	float speed;
+	int speedMode = objGPSIns.speed_mode;
+	/*switch (speedMode)
+	{
+	case 0:
+		nearSpeed = 0;
+		break;
+	case 1:
+		nearSpeed = 0;
+		break;
+	case 2:
+		nearSpeed = 0;
+		break;
+	case 3:
+		nearSpeed = 0;
+		break;
+	case 4:
+		nearSpeed = 0;
+		break;
+	case 5:
+		nearSpeed = 0;
+		break;
+	case 6:
+		nearSpeed = 0;
+		break;
+	case 7:
+		nearSpeed = 0;
+		break;
+	case 8:
+		nearSpeed = 0;
+		break;
+	default:
+		break;
+	}*/
+
+	float objSpeed = objGPSIns.speed;
+	float nowSpeed = nowGPSIns.speed;
+	if (objSpeed >nowSpeed) {
+		speed = nowGPSIns.speed + 1;
+	}
+	else if (objSpeed<nowSpeed)
+	{
+		speed = nowGPSIns.speed - 1;
+	}
+	else
+	{
+		speed = objSpeed;
+	}
+	return speed;
+}
+
+/*
+   检验车与目标轨迹距离是否偏大
+*/
+
+void iv::decition::DecitionMaker::checkDistance(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data,double maxDistance) {
+	  double distanceF = DirectDistance(nowGPSIns.gps_x, nowGPSIns.gps_y, (navigation_data[lastNearPointNum])->gps_x, (navigation_data[lastNearPointNum])->gps_y);	
+	  if (distanceF>maxDistance)
+	  {
+        std::cout << "您已远离目标路线\n" << std::endl;
+	  }
+}
+
+
+string getTime()
+{
+	time_t timep;
+	time(&timep);
+	char tmp[64];
+	strftime(tmp, sizeof(tmp), "%Y-%m-%d %H:%M:%S", localtime(&timep));
+	return tmp;
+}
+
+/*
+	给出GPS决策Decition
+*/
+iv::decition::Decition iv::decition::DecitionMaker::getDecideForGPS(iv::GPS_INS now_gps_ins, const std::vector<iv::GPSData> navigation_data) {
+	Decition gps_decition(new DecitionBasic);
+	//DecitionBasic decitionBasic;
+	int aheadNum;
+	if (now_gps_ins.speed_mode>2)
+	{
+		aheadNum = 48;
+	}
+	else {
+		aheadNum = 48;
+	}
+	origin_x = now_gps_ins.gps_x;
+	origin_y = now_gps_ins.gps_y;
+	adjustOriginPoint(now_gps_ins);
+	
+	if (isFirstRun)
+	{
+		getStartPoint(now_gps_ins, navigation_data);
+		isFirstRun = false;
+	}
+	nearestGpsIns = getNearestGpsIns(now_gps_ins, navigation_data);
+    std::cout << "\nlast :%d\t%f\t%f \n" << lastNearPointNum << now_gps_ins.gps_lat << now_gps_ins.gps_lng << std::endl;
+    std::cout << "nearest index: %d\t%f\t%f\n" << nearestGpsIns.index << nearestGpsIns.gps_lat << nearestGpsIns.gps_lng <<std::endl;
+	if (!checkComplete(now_gps_ins, navigation_data))
+	{
+		objPointNum = lastNearPointNum+aheadNum;
+		if (objPointNum > navigation_data.size()-1)
+			objPointNum = aheadNum -1;
+		objGpsIns = *(navigation_data[objPointNum]);
+		double angle = getDirectCrossAngle(now_gps_ins, objGpsIns);
+
+	//	checkDistance(now_gps_ins, navigation_data, 5000);
+
+		gps_decition->speed = getSpeedObj(now_gps_ins, nearestGpsIns);
+		if (angle>5||angle<-5)
+		{
+			gps_decition->wheel_angle = angle;
+		}
+		else
+		{
+			gps_decition->wheel_angle = 0;
+		}
+		//string time = getTime();
+		//ODS("当前时间: %s\n", lastNearPointNum);
+		gps_decition->wheel_angle = 0 - gps_decition->wheel_angle*40;
+		if (gps_decition->wheel_angle > 4000) {
+			gps_decition->wheel_angle = 4000;
+		}
+		if (gps_decition->wheel_angle < -4000) {
+			gps_decition->wheel_angle = -4000;
+		}
+	}
+	else {
+		gps_decition->wheel_angle = 0;
+		gps_decition->speed = 0;
+	}
+	
+	return gps_decition;
+
+}
+
+
+//string iv::decition::DecitionMaker::getTime()
+// {
+//	     time_t timep;
+//	     time(&timep);
+//	     char tmp[64];
+//	     strftime(tmp, sizeof(tmp), "%Y-%m-%d %H:%M:%S", localtime(&timep));
+//	    return tmp;
+//}
+
+
+/*iv::GPS_INS iv::decition::DecitionMaker::getNearestGpsIns(iv::GPS_INS gps_ins) {
+
+GPS_INS lastNearPoint = vec_Gps_Group[lastNearPointNum];
+
+int count = vec_Gps_Group.size();
+
+if ((lastNearPoint).ins_heading_angle <= 45 || (lastNearPoint).ins_heading_angle >= 315) {
+for (int i = lastNearPointNum; i < count;i++) {
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lat>(gps_ins).gps_lat)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+}
+else if ((lastNearPoint).ins_heading_angle >= 45 && (lastNearPoint).ins_heading_angle <= 135) {
+
+for (int i = lastNearPointNum; i < count;i++)
+{
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lng>(gps_ins).gps_lng)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+
+}
+else if ((lastNearPoint).ins_heading_angle >= 135 && (lastNearPoint).ins_heading_angle <= 225) {
+
+for (int i = lastNearPointNum; i < count;i++)
+{
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lat<(gps_ins).gps_lat)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+
+}
+else if ((lastNearPoint).ins_heading_angle >= 225 && (lastNearPoint).ins_heading_angle <= 315) {
+
+for (int i = lastNearPointNum; i < count;i++)
+{
+GPS_INS objGD = vec_Gps_Group[i];
+
+if ((objGD).gps_lng<(gps_ins).gps_lng)
+{
+lastNearPointNum = i;
+return objGD;
+}
+}
+
+}
+
+return lastNearPoint;
+}*/
+
+
+
+/*iv::decition::Decition iv::decition::DecitionMaker::getDecideForGPS(iv::GPS_INS now_gps_ins) {
+	Decition gps_decition(new DecitionBasic);
+	//DecitionBasic decitionBasic;
+	nearestGpsIns = getNearestGpsIns(now_gps_ins);
+	double angle = getCrossAngle(now_gps_ins, nearestGpsIns);
+
+	gps_decition->speed = getSpeedObj(now_gps_ins, nearestGpsIns);
+	gps_decition->wheel_angle = angle;
+	return gps_decition;
+
+}*/
+
+
+/*void iv::decition::DecitionMaker::readFromText() {
+
+	ifstream infile;
+	infile.open("D:\\auto_car\\gps_trace.txt");
+	if (!infile) cout << "error" << endl;
+	string str;
+	int t1 = 0;
+	
+	
+	while (getline(infile, str))   //按行读取,遇到换行符结束
+	{
+
+		
+		cout << str << endl;
+		GPS_INS gps_Ins;
+
+		while (infile >> str)
+		{
+			// ODS("%s\n", str);
+			
+			cout << str << endl;
+			t1++;
+			if (t1 == 2)
+			{
+				gps_Ins.gps_lng = stod(str);
+			}
+			else if (t1 == 3)
+			{
+				gps_Ins.gps_lat = stod(str);
+			}
+			else if (t1 == 4)
+			{
+				gps_Ins.speed = stod(str);
+			}
+			else if (t1 == 6)
+			{
+				gps_Ins.ins_heading_angle = stod(str);
+				vec_Gps_Group.push_back(gps_Ins);
+				t1 = 0;
+			}
+		}
+		
+		
+	}
+	infile.close();
+}*/
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/decide_gps_00.cpp

@@ -0,0 +1,4684 @@
+#include <decition/decide_gps_00.h>
+#include <decition/adc_tools/compute_00.h>
+#include <decition/adc_tools/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/obs_predict.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+#include <time.h>
+#include <decition/adc_controller/base_controller.h>
+#include <decition/adc_controller/pid_controller.h>
+#include <decition/adc_planner/lane_change_planner.h>
+#include <decition/adc_planner/frenet_planner.h>
+#include <QTime>
+#include <iomanip>
+
+using namespace std;
+
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+#define PI (3.1415926535897932384626433832795)
+#define AVOID_NEW 1
+iv::decition::DecideGps00::DecideGps00() {
+
+}
+iv::decition::DecideGps00::~DecideGps00() {
+
+}
+
+float iv::decition::DecideGps00::xiuzhengCs=0;
+
+int iv::decition::DecideGps00::PathPoint = -1;
+double iv::decition::DecideGps00::minDis = iv::MaxValue;
+double iv::decition::DecideGps00::maxAngle = iv::MinValue;
+//int iv::decition::DecideGps00::lastGpsIndex = iv::MaxValue;
+int iv::decition::DecideGps00::lastGpsIndex = 0;
+double iv::decition::DecideGps00::controlSpeed = 0;
+double iv::decition::DecideGps00::controlBrake = 0;
+double iv::decition::DecideGps00::controlAng = 0;
+double iv::decition::DecideGps00::Iv = 0;
+double iv::decition::DecideGps00::lastU = 0;
+double iv::decition::DecideGps00::lastVt = 0;
+double iv::decition::DecideGps00::lastEv = 0;
+
+int iv::decition::DecideGps00::gpsLineParkIndex = 0;
+int iv::decition::DecideGps00::gpsMapParkIndex = 0;
+double iv::decition::DecideGps00::lastDistance = MaxValue;
+double iv::decition::DecideGps00::lastPreObsDistance = MaxValue;
+double iv::decition::DecideGps00::obsDistance = -1;
+double iv::decition::DecideGps00::obsSpeed=0;
+double iv::decition::DecideGps00::obsDistanceAvoid = -1;
+double iv::decition::DecideGps00::lastDistanceAvoid = -1;
+
+double iv::decition::DecideGps00::lidarDistance = -1;
+double iv::decition::DecideGps00::myesrDistance = -1;
+double iv::decition::DecideGps00::lastLidarDis = -1;
+
+bool iv::decition::DecideGps00::parkMode = false;
+bool iv::decition::DecideGps00::readyParkMode = false;
+
+bool iv::decition::DecideGps00::zhucheMode = false;
+bool iv::decition::DecideGps00::readyZhucheMode = false;
+bool iv::decition::DecideGps00::hasZhuched = false;
+
+double iv::decition::DecideGps00::lastLidarDisAvoid = -1;
+double iv::decition::DecideGps00::lidarDistanceAvoid = -1;
+
+int iv::decition::DecideGps00::finishPointNum = -1;
+int iv::decition::DecideGps00::zhuchePointNum = 0;
+
+///kkcai, 2020-01-03
+bool iv::decition::DecideGps00::isFirstRun = true;
+//////////////////////////////////////////////
+
+float iv::decition::DecideGps00::minDecelerate=0;
+//bool iv::decition::DecideGps00::startingFlag = true;//起步标志，在起步时需要进行frenet规划。
+
+double offset_real = 0;
+double realspeed;
+double dSpeed;
+double dSecSpeed;
+double secSpeed;
+double ac;
+
+
+iv::Point2D obsPoint(-1, -1);
+iv::Point2D obsPointAvoid(-1, -1);
+
+
+int esrIndex = -1;
+int esrIndexAvoid = -1;
+double obsSpeedAvoid = 0;
+
+double esrDistance = -1;
+double esrDistanceAvoid = -1;
+int obsLostTime = 0;
+int obsLostTimeAvoid = 0;
+
+// double avoidTime = 0;
+
+int avoidXNewPre=0,avoidXNewPreFilter=0;
+vector<int> avoidXNewPreVector;
+int avoidXNew=0;
+int avoidXNewLast=0;
+double avoidX =0;
+int    turnLampFlag=0;  //  if <0:left , if >0:right
+float roadWidth = 3.5;
+int roadSum =10;
+int roadNow = 0;
+int roadOri =0;
+int roadPre = -1;
+int lastRoadOri = 0;
+
+int lightTimes = 0;
+
+
+int lastRoadNum=1;
+
+int stopCount = 0;
+
+int gpsMissCount = 0;
+
+bool rapidStop = false;
+
+bool hasTrumpeted = false;
+
+
+bool changeRoad=false;
+//实验手刹
+bool handFirst = true;
+double handTimeSpan = 0;
+double handStartTime = 0;
+double handLastTime = 0;
+bool handPark = false;
+long handParkTime=10000;
+
+//喇叭
+bool trumpetFirstCount=true;
+double trumpetTimeSpan = 0;
+double trumpetStartTime = 0;
+double trumpetLastTime = 0;
+
+//过渡
+bool transferFirstCount=true;
+double transferTimeSpan = 0;
+double transferStartTime = 0;
+double transferLastTime = 0;
+
+bool busMode = false;
+bool parkBesideRoad=false;
+
+bool chaocheMode = false;
+bool specialSignle = false;
+
+double offsetX = 0;
+
+double steerSpeed=9000;
+
+bool transferPieriod;
+bool transferPieriod2;
+double traceDevi;
+
+bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
+bool useFrenet = false;    //标志是否启用frenet算法避障
+bool useOldAvoid = true;   //标志是否用老方法避障
+bool front_car_decide_avoid=true;  //前后车距大于30米，后车允许根据自己障碍物情况避障，否则只能听从前车
+bool road_permit_avoid=false;  //true: 道路允许避障，false:道路不允许避障
+
+std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量
+
+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} vehState,lastVehState;
+
+
+std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
+std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY,gpsTraceMapOri;
+
+std::vector<double> esrDisVector(roadSum, -1);
+std::vector<double> lidarDisVector(roadSum, -1);
+std::vector<double> obsDisVector(roadSum,-1);
+std::vector<double> obsSpeedVector(roadSum, 0);
+
+std::vector<int> obsLostTimeVector(roadSum, 0);
+
+std::vector<double> lastLidarDisVector(roadSum, -1);
+std::vector<double> lastDistanceVector(roadSum, -1);
+
+std::vector<iv::Point2D> traceOriLeft,traceOriRight;
+
+bool qiedianCount = false;
+
+static int obstacle_avoid_flag=0;
+static int front_car_id=-1;
+static int front_car_vector_id=-1;
+static bool final_brake_lock=false,brake_mode=false;
+    std::vector<iv::Point2D>  obsSpline;
+//日常展示
+
+#include <QDateTime>
+
+iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
+                                                                   const std::vector<GPSData> gpsMapLine,
+                                                                   iv::LidarGridPtr lidarGridPtr,
+                                                                   std::vector<iv::Perception::PerceptionOutput> lidar_per,
+                                                                   iv::TrafficLight trafficLight)
+{
+    //   QTime xTime;
+    //    xTime.start();
+    //39.14034649083606 117.0863975920476 20507469.630314853  4334165.046101382 353
+    Decition gps_decition(new DecitionBasic);
+    //    vector<iv::Point2D> fpTraceTmp;
+
+    bool bgroupgrpc = false;
+    qint64 ngrpcvalid = 3000;
+    iv::group::groupinfo xgroupgrpcinfo;
+    if((QDateTime::currentMSecsSinceEpoch() - ServiceCarStatus.mgroupgrpcupdatetime ) < ngrpcvalid)
+    {
+        ServiceCarStatus.mMutexgroupgrpc.lock();
+        xgroupgrpcinfo.CopyFrom(ServiceCarStatus.mgroupgrpcinfo);
+        ServiceCarStatus.mMutexgroupgrpc.unlock();
+        bgroupgrpc = true;
+    }
+    if((bgroupgrpc==true)&&(ServiceCarStatus.curID>1)){
+        for(int i=0;i<xgroupgrpcinfo.mvehinfo_size();i++){
+            if((xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid()+1)==ServiceCarStatus.curID){
+                        front_car_id=xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid();
+                        front_car_vector_id=i;
+            }
+        }
+    }else{
+                        front_car_id=-1;
+    }
+
+    if(front_car_id>0){
+       if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mgpsimu()){
+          front_car.front_car_ins.gps_lat=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lat();
+          front_car.front_car_ins.gps_lng=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lon();
+          front_car.front_car_ins.ins_heading_angle=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().heading();
+          front_car.front_car_ins.speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().speed();
+          GaussProjCal(front_car.front_car_ins.gps_lng,front_car.front_car_ins.gps_lat, &front_car.front_car_ins.gps_x, &front_car.front_car_ins.gps_y);
+          front_car.front_car_dis=GetDistance(now_gps_ins,front_car.front_car_ins);
+       }else{
+          front_car.front_car_dis=500;
+       }
+       if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mcarstate()){
+           front_car.vehState=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mstate();
+           int avoidX_record=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mavoidx();
+           if(front_car.vehState!=0&&avoidX_record!=0)
+                 front_car.avoidX=avoidX_record;
+           else
+                 front_car.avoidX=0;
+           front_car.obs_distance=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsdistance();
+           front_car.obs_speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsspeed();
+       }
+       givlog->debug("decition_brain","front_car_id: %d,FrontState: %d,FrontAvoidX: %d,FrontDis: %f,FrontObs: %f",
+                       front_car_id,front_car.vehState,front_car.avoidX,front_car.front_car_dis,front_car.obs_distance);
+    }
+
+    if(front_car_id>0){
+        if(front_car.front_car_dis>35){
+            front_car_decide_avoid=true;
+        }else if((front_car.front_car_dis<=35)&&(front_car.avoidX==0)){
+            front_car_decide_avoid=false;
+        }else if((front_car.front_car_dis<=35)&&(front_car.avoidX!=0)){
+            front_car_decide_avoid=true;
+        }
+    }else{
+        front_car_decide_avoid=true;
+    }
+
+
+
+
+    //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
+    if(!(useFrenet^useOldAvoid)){
+        useFrenet = false;
+        useOldAvoid = true;
+    }
+
+    //    //如果useFrenet、useOldAvoid两者均为真或均为假，则采用原来的方法
+    //    if(useFrenet&&useOldAvoid || !useFrenet&&!useOldAvoid){
+    //        useFrenet = false;
+    //        useOldAvoid = true;
+    //    }
+
+    //  ServiceCarStatus.group_control=false;
+
+
+
+
+    //    GPS_INS gps= Coordinate_UnTransfer(0,1.5,now_gps_ins);
+    //    now_gps_ins.gps_x=gps.gps_x;
+    //    now_gps_ins.gps_y=gps.gps_y;
+
+    //    GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y,&now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
+
+
+    if (isFirstRun)
+    {
+        initMethods();
+        vehState = normalRun;
+        startAvoid_gps_ins = now_gps_ins;
+        init();
+        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
+                                                          DecideGps00::lastGpsIndex,
+                                                          DecideGps00::minDis,
+                                                          DecideGps00::maxAngle);
+        DecideGps00::lastGpsIndex = PathPoint;
+
+
+        if(ServiceCarStatus.speed_control==true){
+            //Compute00().getDesiredSpeed(gpsMapLine);   //add by zj
+            Compute00().getPlanSpeed(gpsMapLine);
+        }
+
+        //	ServiceCarStatus.carState = 1;
+        //        roadOri = gpsMapLine[PathPoint]->roadOri;
+        //        roadNow = roadOri;
+        //        roadSum = gpsMapLine[PathPoint]->roadSum;
+        //  busMode = false;
+        //  vehState = dRever;
+
+        double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
+        if(dis<15){
+            circleMode=true;  //201200102
+        }else{
+            circleMode=false;
+        }
+
+        //     circleMode = true;
+
+
+        getV2XTrafficPositionVector(gpsMapLine);
+        //        group_ori_gps=*gpsMapLine[0];
+        ServiceCarStatus.bocheMode=false;
+        ServiceCarStatus.daocheMode=false;
+        parkMode=false;
+        readyParkMode=false;
+        finishPointNum=-1;
+        roadNowStart=true;
+        isFirstRun = false;
+        obstacle_avoid_flag=0;
+        avoidXNew=0;
+        avoidXNewLast=0;
+        avoidXNewPre=0;
+        avoidXNewPreFilter=0;
+        gpsMapLine[gpsMapLine.size()-1]->frenet_s=0;
+
+        givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",
+                        0,0);
+    }
+    ServiceCarStatus.mvehState=vehState;
+    ServiceCarStatus.mavoidX=avoidXNew;
+
+
+    if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
+        GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
+        now_gps_ins.gps_x=gpsOffset.gps_x;
+        now_gps_ins.gps_y=gpsOffset.gps_y;
+        GaussProjInvCal(now_gps_ins.gps_x,  now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
+    }
+
+
+
+    //1227
+    //  ServiceCarStatus.daocheMode=true;
+
+    //1220
+    changingDangWei=false;
+
+    minDecelerate=0;
+    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
+        //  int a=0;
+        gps_decition->wheel_angle = 0;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->brake=10;
+        return gps_decition;
+    }
+
+
+
+
+
+    //1220
+
+
+    if(ServiceCarStatus.daocheMode){
+
+        now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
+        if( now_gps_ins.ins_heading_angle>=360)
+            now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
+    }
+    //1220 end
+
+
+
+    //1125 traficc
+    traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
+    traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
+    GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
+
+
+    //xiesi
+    ///kkcai, 2020-01-03
+    //ServiceCarStatus.busmode=true;
+    //ServiceCarStatus.busmode=false;//20200102
+    ///////////////////////////////////////////////////
+
+
+    busMode = ServiceCarStatus.busmode;
+    nearStation=false;
+
+    gps_decition->leftlamp = false;
+    gps_decition->rightlamp = false;
+    //    qDebug("heading is %g",now_gps_ins.ins_heading_angle);
+
+
+    aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
+    aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
+
+    aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
+
+
+
+
+    GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+    is_arrivaled = false;
+
+
+
+
+    //  xiuzhengCs=-0.8;  //1026
+
+    xiuzhengCs=0;
+    //    if (parkMode)
+    //    {
+
+
+    //        handBrakePark(gps_decition,10000,now_gps_ins);
+    //        return gps_decition;
+    //    }
+
+
+
+
+
+    realspeed = now_gps_ins.speed;
+
+    secSpeed = realspeed / 3.6;
+
+
+
+
+    //sidePark
+
+    if(ServiceCarStatus.mnParkType==1){
+
+        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
+            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1){
+                ServiceCarStatus.bocheEnable=1;
+            }else if(bocheEN==0){
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }else{
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+
+
+        if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&
+                vehState!=dRever2  &&  vehState!=dRever3 && vehState!=dRever4&&  vehState!=reverseArr ){
+            if(abs(realspeed)>0.1){
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }else{
+                if(trumpet()>3000){
+                    trumpetFirstCount=true;
+                    vehState=dRever;
+                }
+
+            }
+            //         ServiceCarStatus.bocheMode=false;
+        }
+
+    }
+
+
+    //chuizhiPark
+
+    if(ServiceCarStatus.mnParkType==0){
+        if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr ){
+            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1){
+                ServiceCarStatus.bocheEnable=1;
+            }else if(bocheEN==0){
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }else{
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+
+
+        if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect&&  vehState!=reverseArr ){
+
+            if(abs(realspeed)>0.1){
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }else{
+                if(trumpet()>3000){
+                    trumpetFirstCount=true;
+                    vehState=reverseCar;
+                }
+
+            }
+            //     ServiceCarStatus.bocheMode=false;
+
+        }
+    }
+    // ChuiZhiTingChe
+    if (vehState == reverseCar)
+    {
+        Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+
+        vehState = reversing;
+        qiedianCount=true;
+
+
+    }
+    if (vehState == reversing)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
+        if (dis<2.0)//0.5
+        {
+            vehState = reverseCircle;
+            qiedianCount = true;
+            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+        }
+        else {
+            controlAng = 0;
+            dSpeed = 2;
+            dSecSpeed = dSpeed / 3.6;
+            gps_decition->wheel_angle = 0;
+            gps_decition->speed = dSpeed;
+            obsDistance=-1;
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            //	gps_decition->accelerator = 0;
+            return gps_decition;
+        }
+    }
+
+    if (vehState == reverseCircle)
+    {
+        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))
+            //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
+        {
+            vehState = reverseDirect;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+        }
+        else {
+            if (qiedianCount && trumpet()<0)
+                //         if (qiedianCount && trumpet()<1500)
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+            controlAng = Compute00().bocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng*1.05;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+    if (vehState == reverseDirect)
+    {
+        //	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)
+        {
+
+            qiedianCount=true;
+            vehState=reverseArr;
+            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+            //            gps_decition->accelerator = -3;
+            //            gps_decition->brake = 10;
+            //            gps_decition->torque = 0;
+            gps_decition->wheel_angle=0;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            return gps_decition;
+
+
+
+
+        }
+        else {
+
+            //if (qiedianCount && trumpet()<0)
+            if (qiedianCount && trumpet()<1000)
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+                dSpeed = 1;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+
+
+
+            controlAng = 0;
+
+            gps_decition->wheel_angle = 0;
+
+            return gps_decition;
+        }
+    }
+
+
+
+    if (vehState == reverseArr)
+    {
+        //
+
+        ServiceCarStatus.bocheMode=false;
+        if (qiedianCount && trumpet()<1500)
+        {
+
+            //            gps_decition->brake = 10;
+            //            gps_decition->torque = 0;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            ServiceCarStatus.bocheMode=false;
+        }
+        else
+        {
+            qiedianCount = false;
+            trumpetFirstCount = true;
+            ServiceCarStatus.bocheEnable=0;
+            vehState=normalRun;
+            ServiceCarStatus.mbRunPause=true;
+            ServiceCarStatus.mnBocheComplete=100;
+            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
+        }
+
+
+        gps_decition->wheel_angle = 0 ;
+
+
+
+        return gps_decition;
+
+    }
+
+
+
+    //ceFangWeiBoChe
+
+    if (vehState == dRever)
+    {
+        GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+
+        Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+
+        vehState = dRever0;
+        qiedianCount=true;
+
+
+        std::cout<<"enter side boche mode"<<std::endl;
+
+
+
+    }
+    if (vehState == dRever0)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
+        if (dis<2.0)
+        {
+            vehState = dRever1;
+            qiedianCount = true;
+            cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+        }
+        else {
+            controlAng = 0;
+            dSpeed = 2;
+            dSecSpeed = dSpeed / 3.6;
+            gps_decition->wheel_angle = 0;
+            gps_decition->speed = dSpeed;
+            obsDistance=-1;
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            //	gps_decition->accelerator = 0;
+            return gps_decition;
+        }
+    }
+
+
+    if (vehState == dRever1)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);
+
+        if(dis<2.0)
+        {
+            vehState = dRever2;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+        }
+        else {
+            if (qiedianCount && trumpet()<1000)
+
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+            controlAng = Compute00().dBocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+
+    if (vehState == dRever2)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
+        Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+        Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
+        double xx= (pt1.x-pt2.x);
+        // if(xx>0)
+        if(xx>-0.5)
+        {
+            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);
+            double xxx=ptt.x;
+            double yyy =ptt.y;
+            //            if(xxx<-1.5||xx>1){
+            //                int a=0;
+            //            }
+            vehState = dRever3;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+            cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
+            cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
+
+
+        }
+        else {
+            if (qiedianCount && trumpet()<1000)
+
+            {
+
+                /*  gps_decition->brake = 10;
+                gps_decition->torque = 0;  */
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+
+            gps_decition->wheel_angle = 0 ;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+
+    if (vehState == dRever3)
+    {
+        double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
+        double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
+        if((((angdis<5)||(angdis>355)))&&(dis<10.0))
+        {
+            vehState = dRever4;
+            qiedianCount = true;
+            cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+        }
+        else {
+            if (qiedianCount && trumpet()<1000)
+
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+
+
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            }
+
+            controlAng = 0-Compute00().dBocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng*0.95;
+
+            cout<<"farTpointDistance================"<<dis<<endl;
+
+            return gps_decition;
+        }
+    }
+
+
+
+    if (vehState == dRever4)
+    {
+        //	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)
+        {
+
+            qiedianCount=true;
+            vehState=reverseArr;
+            cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+            //            gps_decition->accelerator = -3;
+            //            gps_decition->brake =10 ;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+            gps_decition->wheel_angle=0;
+            return gps_decition;
+
+
+
+
+        }
+        else {
+
+            //if (qiedianCount && trumpet()<0)
+            if (qiedianCount && trumpet()<1000)
+            {
+
+                //                gps_decition->brake = 10;
+                //                gps_decition->torque = 0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+            else
+            {
+                qiedianCount = false;
+                trumpetFirstCount = true;
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            }
+
+
+
+            controlAng = 0;
+
+            gps_decition->wheel_angle = 0;
+
+            return gps_decition;
+        }
+    }
+
+
+
+    if (vehState == reverseArr)
+    {
+        //
+        ServiceCarStatus.bocheMode=false;
+
+        if (qiedianCount && trumpet()<1500)
+        {
+
+            //            gps_decition->brake = 10;
+            //            gps_decition->torque = 0;
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+        }
+        else
+        {
+            qiedianCount = false;
+            trumpetFirstCount = true;
+            ServiceCarStatus.bocheEnable=0;
+            vehState=normalRun;
+            ServiceCarStatus.mbRunPause=true;
+            ServiceCarStatus.mnBocheComplete=100;
+            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
+        }
+
+
+        gps_decition->wheel_angle = 0 ;
+
+
+
+        return gps_decition;
+
+    }
+
+    obsDistance = -1;
+    esrIndex = -1;
+    lidarDistance = -1;
+
+    obsDistanceAvoid = -1;
+    esrIndexAvoid = -1;
+    roadPre = -1;
+    avoidXNewPre=0;
+    //dSpeed = ServiceCarStatus.mroadmode_vel.mfmode0;
+
+    gpsTraceOri.clear();
+    gpsTraceNow.clear();
+    gpsTraceAim.clear();
+    gpsTraceAvoid.clear();
+    gpsTraceBack.clear();
+    gpsTraceNowLeft.clear();
+    gpsTraceNowRight.clear();
+
+
+
+    if (!isFirstRun)
+    {
+            PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+            if(PathPoint<0){
+                PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+            }
+
+    }
+
+
+    if (PathPoint<0)
+    {
+
+        minDecelerate=-1.0;
+        phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
+        return gps_decition;
+
+    }
+
+    DecideGps00::lastGpsIndex = PathPoint;
+
+//    double brake_distance=200;
+//    brake_distance=max(250,(int)(dSpeed*dSpeed+150));
+
+
+    int nmapsize = gpsMapLine.size();
+
+
+    double acc_end = 0.1;
+    static double distoend=1000;
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+                if(PathPoint+1000>gpsMapLine.size()){
+                    distoend=computeDistToEnd(gpsMapLine,PathPoint);
+                }else{
+                    distoend=1000;
+                }
+                if(!circleMode && distoend<50){
+                        double nowspeed = realspeed/3.6;
+                        if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
+                        {
+                            if(distoend == 0.0)distoend = 0.09;
+                            acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                            if(acc_end<(-3.0))acc_end = -3.0;
+                        }
+
+                        if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = -0.5;
+                }
+    }else{
+//                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
+//                        minDecelerate=-0.5;
+//                        std::cout<<"map finish brake"<<std::endl;
+//                }
+                if(!circleMode){
+                        double forecast_final=secSpeed*secSpeed+5;
+                        int forecast_final_point=((int)forecast_final)*10+1500;
+                        static int BrakePoint=-1;
+                        static bool final_brake=false;
+                        static double distance_to_end=1000;
+                        if(PathPoint+forecast_final_point>gpsMapLine.size())
+                        {                           
+                            distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);
+                            givlog->debug("decition_brain","distoend: %f",
+                                            distance_to_end);
+                            if((forecast_final>=distance_to_end)&&(realspeed>3)){
+                                                        final_brake=true;
+                                                        if(BrakePoint==-1)
+                                                             BrakePoint=PathPoint-10;
+                            }
+                        }else{
+                            distance_to_end=1000;
+                        }
+                        if(PathPoint<BrakePoint)
+                        {
+                            final_brake=false;
+                            final_brake_lock=false;
+                            brake_mode=false;
+                            BrakePoint=-1;
+                        }
+                        if(final_brake==true){
+                                if((realspeed>3)&&(final_brake_lock==false)){
+                                            minDecelerate=-0.7;
+                                }else{
+                                            dSpeed=min(dSpeed, 3.0);
+                                            final_brake_lock=true;
+                                            brake_mode=true;
+                                            if(distance_to_end<0.8){
+                                                            minDecelerate=-0.7;
+                                            }
+
+                                }
+                        }
+                }
+    }
+
+    if(!ServiceCarStatus.inRoadAvoid){
+        roadOri = gpsMapLine[PathPoint]->roadOri;
+        roadSum = gpsMapLine[PathPoint]->roadSum;      
+
+    }else{
+        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
+        roadSum = gpsMapLine[PathPoint]->roadSum*3;
+    }
+
+//    roadOri =0;
+//    roadSum =2;
+
+    if(roadNowStart){
+        roadNow=roadOri;
+        roadNowStart=false;
+    }
+
+
+    if(ServiceCarStatus.avoidObs){
+         gpsMapLine[PathPoint]->runMode=1;
+    }else{
+         gpsMapLine[PathPoint]->runMode=0;
+    }
+
+#ifdef AVOID_NEW
+    if(obstacle_avoid_flag==1){
+
+    }else{
+        avoidXNew=0;
+        roadNow = roadOri;
+        if(vehState==backOri){
+            vehState=normalRun;
+        }
+    }
+    givlog->debug("decition_brain","avoidXNew: %d",
+                    avoidXNew);
+#else
+    if(obstacle_avoid_flag==1){
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+    }else{
+        avoidX=0;
+        roadNow = roadOri;
+        if(vehState==backOri){
+            vehState=normalRun;
+        }
+    }
+#endif
+
+    if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
+                                                ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
+    {
+        vehState = normalRun;
+        roadNow = roadOri;
+    }
+
+    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+    gpsTraceMapOri=gpsTraceOri;
+
+    if((vehState == avoiding)&&(obstacle_avoid_flag == 1)){
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+        if((now_s_d.s+20>(gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s))&&((gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s)<((*gpsMapLine[gpsMapLine.size()-1]).frenet_s))){
+            double sPathFinal=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;
+            if(sPathFinal+20<=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s){
+                sPathFinal=sPathFinal+20;
+            }else{
+                sPathFinal=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s;
+            }
+            for(double s=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;s<=sPathFinal;s+=0.1){
+                iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s,-avoidXNew,PathPoint);
+                gpsTraceAvoidXY.push_back(gpsTracePoint);
+            }
+        }
+    }
+
+    if((vehState == avoiding || vehState == backOri)&&(gpsTraceAvoidXY.size()>0)&&(obstacle_avoid_flag==1))
+    {
+        gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
+    }
+
+    if(gpsTraceOri.empty()){
+        gps_decition->wheel_angle = 0;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->brake=10;
+        return gps_decition;
+    }
+
+
+
+    traceDevi=gpsTraceOri[0].x;
+
+    //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
+    //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
+    //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
+    //        startingFlag = false;
+    //    }
+    startingFlag = false;
+    if(startingFlag){
+        //        gpsTraceAim = gpsTraceOri;
+        if(abs(gpsTraceOri[0].x)>1){
+            cout<<"frenetPlanner->getPath:pre"<<endl;
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
+            if(gpsTraceNow.size()==0){
+                gps_decition->accelerator = 0;
+                gps_decition->brake=10;
+                gps_decition->wheel_angle = lastAngle;
+                gps_decition->speed = 0;
+                return gps_decition;
+            }
+        }else{
+            startingFlag = false;
+        }
+    }
+
+
+    if(useFrenet){
+        //如果车辆在变道中，启用frenet规划局部路径
+        if(vehState == avoiding || vehState == backOri){
+           // avoidX = (roadOri - roadNow)*roadWidth;
+             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
+            if(gpsTraceNow.size()==0){
+                gps_decition->accelerator = 0;
+                gps_decition->brake=10;
+                gps_decition->wheel_angle = lastAngle;
+                gps_decition->speed = 0;
+                return gps_decition;
+            }
+        }
+    }
+
+#ifdef AVOID_NEW
+    if(gpsTraceNow.size()==0){
+        if (avoidXNew==0)
+        {
+            if((vehState == backOri)||(vehState == avoiding))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else{
+            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+            gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+            gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
+        }else
+        {
+            if((vehState == avoiding)||(vehState == backOri))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }else{
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+        }
+    }
+#else
+    if(gpsTraceNow.size()==0){
+        if (roadNow==roadOri)
+        {
+            if(vehState == backOri)
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else{
+            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+            gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+            gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
+        }else
+        {
+            if((vehState == avoiding)||(vehState == backOri))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+//                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+//                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }else{
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+//                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+//                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+        }
+    }
+
+#endif
+
+
+
+    if(vehState==normalRun)
+    {
+        if(gpsTraceNow.size()>200){
+            if(gpsTraceNow[200].x<-3){
+                gps_decition->leftlamp = true;
+                gps_decition->rightlamp = false;
+            }else if(gpsTraceNow[200].x>3){
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = true;
+            }else{
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = false;
+            }
+        }
+    }
+
+    //  dSpeed = getSpeed(gpsTraceNow);
+    dSpeed =80;
+
+    planTrace.clear();//Add By YuChuli, 2020.11.26
+    for(int i=0;i<gpsTraceNow.size();i++){
+        TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
+        planTrace.push_back(pt);
+    }
+
+    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK())){
+        if(!circleMode && PathPoint>(gpsMapLine.size()-20)){
+            controlAng=0;
+        }
+    }else{
+        if(!circleMode && PathPoint>(gpsMapLine.size()-250)){
+           if(realspeed<0.5)
+                controlAng=0;
+        }
+    }
+
+    //1220
+    if(ServiceCarStatus.daocheMode){
+        controlAng=0-controlAng;
+    }
+
+    //2020-0106
+    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
+
+        if(vehState==avoiding){
+            ServiceCarStatus.msysparam.vehWidth=2.4;
+            controlAng=max(-150.0,controlAng);//35  zj-150
+            controlAng=min(150.0,controlAng);//35   zj-150
+        }
+        if(vehState==backOri){
+            controlAng=max(-150.0,controlAng);//35   zj-150
+            controlAng=min(150.0,controlAng);//35     zj-150
+        }
+    }
+
+//    givlog->debug("brain_decition","vehState: %d,controlAng: %f",
+//            vehState,controlAng);
+
+
+    //准备驻车
+    if (readyZhucheMode)
+    {
+
+        cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+        cout<<"zhuche point : "<<zhuchePointNum<<endl;
+
+        double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
+
+        if (dis<35)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
+            double zhucheDistance = pt.y;
+#if 0
+
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+
+
+            if (zhucheDistance < 20 && dis < 25)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#else
+            if (dSpeed > 12)
+            {
+                dSpeed = 12;
+            }
+
+
+
+            if (zhucheDistance < 9 && dis < 9)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#endif
+
+
+            if (zhucheDistance < 3.0 && dis < 3.5)
+            {
+                dSpeed = min(dSpeed, 5.0);
+                zhucheMode = true;
+                readyZhucheMode = false;
+                cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+
+            }
+
+        }
+
+    }
+
+
+
+
+
+    if (readyParkMode)
+    {
+        double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
+        hasZhuched = true;
+
+        if (parkDis < 25)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
+            double parkDistance = pt.y;
+
+
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+            //dSpeed = 15;//////////////////////////////
+
+            if (parkDistance < 15 && parkDistance >= 4.5 && parkDis<20)
+            {
+                dSpeed = min(dSpeed, 8.0);
+            }else if (parkDistance < 4.5 && parkDistance >= 3.5 && parkDis<5.0){
+                dSpeed = min(dSpeed, 5.0);
+            }else if(parkDistance < 3.5 && parkDis<4.0){
+                dSpeed = min(dSpeed, 3.0);
+            }
+
+
+            //            float stopDis=2;
+            //            if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+            //                stopDis=1.6;
+            //            } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
+            //                stopDis=1.8;
+            //            }
+
+            if (parkDistance < 1.6  && parkDis<2.0)
+            {
+                // dSpeed = 0;
+                parkMode = true;
+                readyParkMode = false;
+            }
+
+
+        }
+    }
+
+    if(ServiceCarStatus.speed_control==true){
+            dSpeed = min(doubledata[PathPoint][4],dSpeed);
+    }
+    else
+    {
+            if (gpsMapLine[PathPoint]->roadMode ==0)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = false;
+
+            }else  if (gpsMapLine[PathPoint]->roadMode == 5)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = false;
+
+            }else if (gpsMapLine[PathPoint]->roadMode == 11)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
+
+            }else if (gpsMapLine[PathPoint]->roadMode == 14)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
+                gps_decition->leftlamp = true;
+                gps_decition->rightlamp = false;
+            }else if (gpsMapLine[PathPoint]->roadMode == 15)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = true;
+            }else if (gpsMapLine[PathPoint]->roadMode == 16)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
+                gps_decition->leftlamp = true;
+                gps_decition->rightlamp = false;
+            }else if (gpsMapLine[PathPoint]->roadMode == 17)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = true;
+            }else if (gpsMapLine[PathPoint]->roadMode == 18)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
+            }else if (gpsMapLine[PathPoint]->roadMode == 1)
+            {
+                dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
+            }else if (gpsMapLine[PathPoint]->roadMode == 2)
+            {
+                dSpeed = min(15.0,dSpeed);
+            }
+            else if (gpsMapLine[PathPoint]->roadMode == 7)
+            {
+                dSpeed = min(15.0,dSpeed);
+                xiuzhengCs=1.5;
+            }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
+            {
+                dSpeed = min(4.0,dSpeed);
+
+            }else{
+                    dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+                    gps_decition->leftlamp = false;
+                    gps_decition->rightlamp = false;
+
+            }
+    }
+
+    if (gpsMapLine[PathPoint]->speed_mode == 2)
+    {
+        dSpeed = min(25.0,dSpeed);
+
+    }
+#ifdef AVOID_NEW  //20220223toggle
+//    if((vehState==avoiding)||(vehState==preAvoid))
+//    {
+//        dSpeed = min(8.0,dSpeed);
+//    }else if((vehState==backOri)||(avoidXNew!=0)){
+//        dSpeed = min(12.0,dSpeed);
+//    }
+#else
+    if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
+    {
+        dSpeed = min(8.0,dSpeed);
+    }
+#endif
+
+    if((gpsMapLine[PathPoint]->speed)>0.001)
+    {
+        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
+//        if((gpsMapLine[PathPoint]->speed)>4.5)
+//        {
+//            dSpeed =gpsMapLine[PathPoint]->speed*3.6;
+//        }
+    }
+    //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
+    if(brake_mode==true){
+        dSpeed=min(dSpeed, 3.0);
+    }
+
+    if(front_car_id>0){
+        static bool brake_state=false;
+        if(((front_car.vehState!=0)||(front_car.avoidX!=0))&&(front_car.front_car_dis<15)){
+            brake_state=true;
+        }
+        if(brake_state==true){
+            dSpeed=min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
+        }
+        if(((front_car.vehState==0)&&(front_car.avoidX==0))||(front_car.front_car_dis>25)){
+            brake_state=false;
+        }
+
+    }
+
+    dSpeed = limitSpeed(controlAng, dSpeed);
+    dSecSpeed = dSpeed / 3.6;
+//    givlog->debug("brain_decition_speed","dspeed: %f",
+//                  dSpeed);
+
+
+    if(vehState==changingRoad){
+        if(gpsTraceNow[0].x>1.0){
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }else if(gpsTraceNow[0].x<-1.0){
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }else{
+            vehState==normalRun;
+        }
+    }
+
+    static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
+    static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
+    if(!ServiceCarStatus.daocheMode){
+        //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
+
+
+        if(obs_filter_flag==0)
+        {
+            if(obsDistance>0&&obsDistance<60)
+            {
+                    obs_filter++;
+                    if(obs_filter<20)                          //80
+                    {
+                        obsDistance=-1;
+                        obsSpeed=0;
+                    }
+                    else
+                    {
+                        obs_filter_flag=1;
+                        obs_filter_dis_memory=obsDistance;
+                        obs_filter_speed_memory=obsSpeed;
+                        obs_filter=0;
+                    }
+            }else{
+                obs_filter=0;
+            }
+        }else
+        {
+            if(obsDistance<0||obsDistance>60)
+            {
+                obs_filter++;
+                if(obs_filter<20)                           //80
+                {
+                    obsDistance=obs_filter_dis_memory;
+                    obsSpeed=obs_filter_speed_memory;
+                }
+                else
+                {
+                    obs_filter_flag=0;
+                    obs_filter=0;
+                }
+            }else
+            {
+                obs_filter=0;
+                obs_filter_dis_memory=obsDistance;
+                obs_filter_speed_memory=obsSpeed;
+            }
+        }
+
+
+        obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
+
+
+    }else{
+        gpsTraceRear.clear();
+        for(int i=0;i<gpsTraceNow.size();i++){
+            Point2D pt;
+            pt.x=0-gpsTraceNow[i].x;
+            pt.y=0-gpsTraceNow[i].y;
+            gpsTraceRear.push_back(pt);
+        }
+        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        //  obsDistance=-1; //1227
+    }
+    ServiceCarStatus.mObsDistance=obsDistance;
+    ServiceCarStatus.mObsSpeed=obs_speed_for_avoid;
+
+    //group map end park pro--------start
+    if(front_car_id>0){
+        static  bool final_lock=false;
+        if((distoend<90)&&(front_car.front_car_dis<obsDistance+10)){
+            if((obsDistance>3)&&(obsDistance<20)){
+                if((realspeed>3)&&(final_lock==false)){
+                    minDecelerate=-0.7;
+                }else{
+                    dSpeed = min(3.0,dSpeed);
+                    final_lock=true;
+                }
+                obsDistance=200;
+            }else if((obsDistance>1)&&(obsDistance<3)){
+                minDecelerate=-0.5;
+                obsDistance=200;
+            }else if(obsDistance<1){
+                minDecelerate=-1.0;
+            }
+            if(realspeed<1){
+                final_lock=false;
+            }
+        }
+    }
+    //group map end park pro-----------end
+
+    //old_bz--------------------------------------------------------------------------------------------------
+    if (vehState == avoiding)
+    {
+//        double avoidDistance=GetDistance(startAvoidGpsIns,now_gps_ins);   //Toggle 20220223
+//        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+//        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+//            vehState = normalRun;
+//            //            useFrenet = false;
+//            //            useOldAvoid = true;
+//        }
+//        else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
+//            // vehState = avoidObs; 0929
+//            vehState = normalRun;
+//            turnLampFlag=0;
+//        }
+//        else if (turnLampFlag>0)
+//        {
+//            gps_decition->leftlamp = false;
+//            gps_decition->rightlamp = true;
+//        }
+//        else if(turnLampFlag<0)
+//        {
+//            gps_decition->leftlamp = true;
+//            gps_decition->rightlamp = false;
+//        }
+
+        dSpeed = min(6.0,dSpeed);
+        if (turnLampFlag>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if(turnLampFlag<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+
+    if (vehState==preBack)
+    {
+        vehState = backOri;
+    }
+
+
+    if (vehState==backOri)
+    {
+        double backDistance=GetDistance(startBackGpsIns,now_gps_ins);
+        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+            vehState = normalRun;
+            turnLampFlag=0;
+            //            useFrenet = false;
+            //            useOldAvoid = true;
+        }
+        else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
+            // vehState = avoidObs; 0929
+            vehState = normalRun;
+            obstacle_avoid_flag=0;
+        }
+        else if (turnLampFlag>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (turnLampFlag<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+
+    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
+    //   计算回归原始路线
+#ifdef AVOID_NEW
+    if((avoidXNew == 0)&&(vehState == avoiding)){
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+        if(fabs(now_s_d.d)<0.05){
+            vehState=normalRun;
+            obstacle_avoid_flag=0;
+        }
+    }
+
+
+
+
+//    int avoidXPre=20;   //20220223 Toggle
+//    if (avoidXNew!=0)
+//    {
+//        if(useOldAvoid){
+//            int avoidLeft_value=0;
+//            int avoidRight_value=0;
+//            int* avoidLeft_p=&avoidLeft_value;
+//            int* avoidRight_p=&avoidRight_value;
+//            computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+//            avoidXPre=computeBackDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNew);
+//            givlog->debug("decition_brain","vehState: %d,roadOri: %d,roadSum: %d,vehWidth: %f,avoidLeft: %d,avoidRight_value: %d,avoidXNewPre: %d",
+//                            vehState,roadOri,roadSum,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXPre);
+//        }
+//    }
+//    if ((avoidXNew!=0 && avoidXPre!=20))
+//    {
+
+//        if((avoidXPre-avoidXNew)<0)
+//            turnLampFlag=-1;
+//        else if((avoidXPre-avoidXNew)>0)
+//            turnLampFlag=1;
+//        else
+//            turnLampFlag=0;
+
+
+//        //double back_offset=avoidXPre-avoidXNew;
+//        if(useOldAvoid){
+//            gpsTraceAvoidXY.clear();
+//            gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidXPre,now_gps_ins,gpsTraceNow);
+//            startBackGpsIns = now_gps_ins;
+//        }
+//        vehState = backOri;
+//        avoidXNew=0;
+//        hasCheckedBackLidar = false;
+
+//    }
+#else
+    if ((roadNow!=roadOri))
+    {
+        if(useFrenet){
+            if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
+            {
+                double offsetL = -(roadSum - 1)*roadWidth;
+                double offsetR = (roadOri - 0)*roadWidth;
+                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
+            }
+        }
+        else if(useOldAvoid){
+            roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
+          //  avoidX = (roadOri - roadNow)*roadWidth; //1012
+             avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+        }
+    }
+    if ((roadNow != roadOri && roadPre!=-1))
+    {
+
+        roadNow = roadPre;
+   //     avoidX = (roadOri - roadNow)*roadWidth;
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+
+        if(avoidX<0)
+            turnLampFlag<0;
+        else if(avoidX>0)
+            turnLampFlag>0;
+        else
+            turnLampFlag=0;
+
+        if(useOldAvoid){
+            //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+            gpsTraceAvoidXY.clear();
+            gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidX,now_gps_ins,gpsTraceNow);
+            startBackGpsIns = now_gps_ins;
+        }
+        else if(useFrenet){
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+        }
+
+        vehState = backOri;
+        hasCheckedBackLidar = false;
+        //  checkBackObsTimes = 0;
+
+        cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
+
+    }
+#endif
+
+
+    static bool avoid_speed_flag=false;
+    /*if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
+            (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
+            && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
+    {
+        minDecelerate=-0.4;
+        avoid_speed_flag=true;
+    }*/
+
+    road_permit_avoid=false;
+    if(PathPoint+400<gpsMapLine.size()){
+        int road_permit_sum=0;
+        for(int k=PathPoint;k<PathPoint+400;k++){
+                if((gpsMapLine[k]->mbnoavoid==false)&&(gpsMapLine[k]->roadSum>1))
+                                 road_permit_sum++;
+        }
+        if(road_permit_sum>=400)
+            road_permit_avoid=true;
+    }
+
+    //shiyanbizhang 0929
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>1)//&& (avoid_speed_flag==true)        //
+            &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
+            && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
+//        ObsTimeStart=GetTickCount();
+        ObsTimeEnd+=1.0;
+        //dSpeed = min(6.0,dSpeed);
+        cout<<"\n====================preAvoid time count start==================\n"<<endl;
+    }
+//    if(ObsTimeStart!=-1){
+//        ObsTimeEnd=GetTickCount();
+//    }
+//    if(ObsTimeEnd!=-1){
+//        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
+//    }
+
+
+
+//    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+//        vehState=avoidObs;
+//        ObsTimeStart=-1;
+//        ObsTimeEnd=-1;
+//        ObsTimeSpan=-1;
+//        avoid_speed_flag=false;
+//        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+//    }
+
+
+
+
+//    if((ObsTimeStart!=-1)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)){
+//            ObsTimeEnd+=1.0;
+//    }
+
+    if(ObsTimeEnd>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+        vehState=avoidObs;
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+        avoid_speed_flag=false;
+        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+    }
+
+    if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)||(road_permit_avoid==false)||(front_car_decide_avoid==false)){
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+        avoid_speed_flag=false;
+    }
+
+    //避障模式
+    if (vehState == avoidObs   || vehState==waitAvoid ) {
+
+        //      if (obsDistance <20 && obsDistance >0)
+        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
+        {
+            dSpeed = min(6.0,dSpeed);
+            avoidTimes++;
+            //          if (avoidTimes>=6)
+            if (avoidTimes>=ServiceCarStatus.aocfTimes)
+            {
+                vehState = preAvoid;
+                avoidTimes = 0;
+            }
+
+        }
+        //        else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
+        //        {
+        //            dSpeed = 10;
+        //            avoidTimes = 0;
+        //        }
+        else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
+        {
+            dSpeed =  min(15.0,dSpeed);
+            avoidTimes = 0;
+        }
+        else
+        {
+            avoidTimes = 0;
+        }
+
+    }
+
+    //test add 20220223
+//            int avoidLeft_value=0;
+//            int avoidRight_value=0;
+//            int* avoidLeft_p=&avoidLeft_value;
+//            int* avoidRight_p=&avoidRight_value;
+//            computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+//            avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNewLast);
+
+//                avoidXNew=avoidXNewPre;
+//                if(avoidXNew<0)
+//                    turnLampFlag=-1;
+//                else if(avoidXNew>0)
+//                    turnLampFlag=1;
+//                else
+//                    turnLampFlag=0;
+//  givlog->debug("decition_brain","roadOri: %d,roadSum: %d,roadWidth: %f,carWidth: %f,leftBoundary: %d,rightBoundary: %d,avoidXNewLast: %d,avoidXNewPre: %d,avoidXNew: %d",
+//                  roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXNewLast,avoidXNewPre,avoidXNew);
+
+//    obsSpline.clear();
+//    Point2D obs(0,0);
+//    obs.s=15;
+//    obs.d=0;
+//    obsSpline.push_back(obs);
+//    Point2D obs1(0,0);
+//    obs1.s=500;
+//    obs1.d=-1;
+//    obsSpline.push_back(obs1);
+//    Point2D obs2(0,0);
+//    obs2.s=500;
+//    obs2.d=-2;
+//    obsSpline.push_back(obs2);
+//    Point2D obs3(0,0);
+//    obs3.s=500;
+//    obs3.d=-3;
+//    obsSpline.push_back(obs3);
+//    Point2D obs4(0,0);
+//    obs4.s=500;
+//    obs4.d=-4;
+//    obsSpline.push_back(obs4);
+//    avoidXNew=-2;
+//    avoidXNewLast=0;
+    //double start=GetTickCount();
+//    gpsTraceNow.clear();
+//    gpsTraceAvoidXY.clear();
+//    gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//    std::cout<<"===================spline========================"<<gpsTraceAvoidXY.size()<<       std::endl;
+//    vehState = avoiding;
+//    obstacle_avoid_flag=1;
+//    hasCheckedAvoidLidar = false;
+    //avoidXNewLast=avoidXNew;
+   //test end
+
+
+//double period=end-start;
+//std::cout<<"===================period========================"<<period<<       std::endl;
+
+
+
+
+
+
+    if((vehState == preAvoid)||(avoidXNew!=0))
+    {
+        int avoidLeft_value=0;
+        int avoidRight_value=0;
+        int* avoidLeft_p=&avoidLeft_value;
+        int* avoidRight_p=&avoidRight_value;
+        computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+        avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNewLast);
+        if(avoidXNewPreVector.size()<5){
+            avoidXNewPreVector.push_back(avoidXNewPre);
+        }else{
+            if(avoidXNewPreVector[0]!=avoidXNewLast){
+                for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+                    if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+                        avoidXNewPreFilter=avoidXNewLast;
+                        break;
+                    }
+                    if(i==avoidXNewPreVector.size()-1)
+                        avoidXNewPreFilter=avoidXNewPreVector[0];
+                }
+            }
+            avoidXNewPreVector.clear();
+        }
+        if(avoidXNewPreFilter!=avoidXNewLast){
+            avoidXNew=avoidXNewPre;
+            if(avoidXNew<0)
+                turnLampFlag=-1;
+            else if(avoidXNew>0)
+                turnLampFlag=1;
+            else
+                turnLampFlag=0;
+
+           gpsTraceNow.clear();
+           gpsTraceAvoidXY.clear();
+           givlog->debug("decition_brain","avoidXNew: %d,avoidXNewLast: %d",
+                           avoidXNew,avoidXNewLast);
+           //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+           gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+           vehState = avoiding;
+           startAvoidGpsIns = now_gps_ins;
+           obstacle_avoid_flag=1;
+           hasCheckedAvoidLidar = false;
+           avoidXNewLast=avoidXNew;
+           avoidXNewPreFilter=avoidXNew;
+        }
+    }
+
+
+
+//    if (vehState == preAvoid)
+//    {
+//        cout<<"\n====================preAvoid==================\n"<<endl;
+//        /* if (obsDisVector[roadNow]>30)*/  //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
+//        if (obsDistance>ServiceCarStatus.aocfDis)
+//        {
+//            // vehState = avoidObs; 0929
+//            vehState=normalRun;
+//        }
+//        else
+//        {
+//            //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
+//            if(useOldAvoid){
+//#ifdef AVOID_NEW
+//                int avoidLeft_value=0;
+//                int avoidRight_value=0;
+//                int* avoidLeft_p=&avoidLeft_value;
+//                int* avoidRight_p=&avoidRight_value;
+//                computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+//                //computeAvoidBoundary(0,2,4.5,2.4,avoidLeft_p,avoidRight_p);
+//                //avoidLeft_value=-5;
+//                //avoidRight_value=0;
+//                avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value);
+//                givlog->debug("decition_brain","vehState: %d,roadOri: %d,roadSum: %d,vehWidth: %f,avoidLeft: %d,avoidRight_value: %d,avoidXNewPre: %d",
+//                                vehState,roadOri,roadSum,ServiceCarStatus.msysparam.vehWidth,avoidLeft_value,avoidRight_value,avoidXNewPre);
+//                givlog->debug("decition_brain","avoidXNewPre1: %d",avoidXNewPre);
+
+//#else
+//                roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
+//              //  avoidX = (roadOri - roadNow)*roadWidth;  //1012
+//                 avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+
+//#endif
+//            }
+//            else if(useFrenet){
+//                double offsetL = -(roadSum - 1)*roadWidth;
+//                double offsetR = (roadOri - 0)*roadWidth;
+//                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
+//            }
+
+
+
+//#ifdef AVOID_NEW
+//            if (avoidXNewPre ==0 && obsDistance<1.5 && obsDistance>0)
+//            {
+//                vehState = avoidObs;
+//            }
+//            else if (avoidXNewPre != 0)
+//            {
+//                if(useOldAvoid){
+//                     avoidXNew=avoidXNewPre;
+//                     if(avoidXNew<0)
+//                         turnLampFlag<0;
+//                     else if(avoidXNew>0)
+//                         turnLampFlag>0;
+//                     else
+//                         turnLampFlag=0;
+
+//                    gpsTraceNow.clear();
+//                    gpsTraceAvoidXY.clear();
+//                    //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//                    gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,0);
+//                    startAvoidGpsIns = now_gps_ins;
+//                }
+//                vehState = avoiding;
+//                obstacle_avoid_flag=1;
+//                hasCheckedAvoidLidar = false;
+//                avoidXNewLast=avoidXNew;
+//            }
+//#else
+//            if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
+//            {
+//                //  vehState = waitAvoid; 0929
+//                vehState = avoidObs;
+//            }
+//            else if (roadPre != -1)
+//            {
+//                if(useOldAvoid){
+//                    roadNow = roadPre;
+//                  //  avoidX = (roadOri - roadNow)*roadWidth;
+//                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+
+//                     if(avoidX<0)
+//                         turnLampFlag<0;
+//                     else if(avoidX>0)
+//                         turnLampFlag>0;
+//                     else
+//                         turnLampFlag=0;
+
+//                    gpsTraceNow.clear();
+//                    //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+//                    gpsTraceAvoidXY.clear();
+//                    gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidX,now_gps_ins);
+//                    startAvoidGpsIns = now_gps_ins;
+//                }
+//                else if(useFrenet){
+//                    if(roadPre != roadNow){
+//                        avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+//                        startAvoidGpsInsVector[roadNow] = now_gps_ins;
+//                    }
+//                    roadNow = roadPre;
+//                //    avoidX = (roadOri - roadNow)*roadWidth;
+//                     avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+//                    gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+//                }
+
+
+//                vehState = avoiding;
+//                obstacle_avoid_flag=1;
+
+//                hasCheckedAvoidLidar = false;
+
+//                cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;
+
+//            }
+//#endif
+//        }
+//    }
+
+    //--------------------------------------------------------------------------------old_bz_end
+
+
+    if (parkMode)
+    {
+        minDecelerate=-1.0;
+
+        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
+            parkMode=false;
+        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>20){
+            parkMode=false;
+        }
+
+    }
+
+
+
+    //驻车
+
+    if (zhucheMode)
+    {
+        int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
+
+        //        if(trumpet()<16000)
+        if (trumpet()<mzcTime)
+        {
+            //            if (trumpet()<12000){
+            cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
+            minDecelerate=-1.0;
+            if(abs(realspeed)<0.2){
+                controlAng=0;  //tju
+            }
+        }
+        else
+        {
+            hasZhuched = true; //1125
+            zhucheMode = false;
+            trumpetFirstCount = true;
+            cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
+        }
+
+    }
+
+
+    //判断驻车标志位
+    if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
+    {
+        hasZhuched = false;
+    }
+
+    if ( vehState==changingRoad || vehState==chaocheBack)
+    {
+        double lastAng = 0.0 - lastAngle;
+
+        if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
+
+            if (controlAng>40)
+            {
+                controlAng =40;
+            }
+            else if (controlAng<-40)
+            {
+                controlAng = - 40;
+            }
+        }
+
+    }
+
+
+    //速度结合角度的限制
+    controlAng = limitAngle(realspeed, controlAng);
+
+
+    std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
+
+
+
+    //1220
+    if(PathPoint+80<gpsMapLine.size()-1){
+        if(gpsMapLine[PathPoint+80]->roadMode==4  && !ServiceCarStatus.daocheMode){
+            changingDangWei=true;
+        }
+    }
+
+    if(changingDangWei){
+        if(abs(realspeed)>0.1){
+            dSpeed=0;
+        }else{
+            dSpeed=0;
+            gps_decition->dangWei=2;
+            ServiceCarStatus.daocheMode=true;
+        }
+    }
+    //1220 end
+
+
+
+
+    for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
+    {
+        GPS_INS gpsIns;
+        GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude,  &gpsIns.gps_x, &gpsIns.gps_y);
+
+        double dis = GetDistance(gpsIns, now_gps_ins);
+        if(dis<20)
+            ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
+    }
+
+
+
+
+    if (ServiceCarStatus.carState == 0 && busMode)
+    {
+
+        minDecelerate=-1.0;
+    }
+
+
+
+    if (ServiceCarStatus.carState == 3 && busMode)
+    {
+
+        if(realspeed<0.1){
+            ServiceCarStatus.carState=0;
+            minDecelerate=-1.0;
+        }else{
+            nearStation=true;
+            dSpeed=0;
+        }
+
+    }
+
+    ///kkcai, 2020-01-03
+    //if (ServiceCarStatus.carState == 2 && busMode)
+    if (ServiceCarStatus.carState == 2)
+    {
+
+        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
+        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
+        //   gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
+        qDebug("lat:%f", aim_gps_ins.gps_lat);
+        qDebug("lon:%f", aim_gps_ins.gps_lng);
+
+        std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
+        double dis = GetDistance(aim_gps_ins, now_gps_ins);
+        Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
+
+        std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
+        std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;
+
+        //         if (dis<20 && pt.y<8&& realspeed<0.1)
+        if (dis<20 && pt.y<5 && abs(pt.x)<3)
+        {
+            dSpeed = 0;
+            nearStation=true;
+            //is_arrivaled = true;
+            //ServiceCarStatus.status[0] = true;
+            ServiceCarStatus.istostation=1;
+            minDecelerate=-1.0;
+        }
+
+        else if (dis<20 && pt.y<15 && abs(pt.x)<3)
+        {
+            nearStation=true;
+            dSpeed = min(8.0, dSpeed);
+        }
+        else if (dis<30 && pt.y<20 && abs(pt.x)<3)
+        {
+            dSpeed = min(15.0, dSpeed);
+        }
+
+        else if (dis<50 && abs(pt.x)<3)
+        {
+            dSpeed = min(20.0, dSpeed);
+        }
+    }
+
+
+    dSecSpeed = dSpeed / 3.6;
+    gps_decition->speed = dSpeed;
+
+
+
+    if (gpsMapLine[PathPoint]->runMode == 2)
+    {
+        obsDistance = -1;
+
+    }
+
+    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
+
+    //-------------------------------traffic light----------------------------------------------------------------------------------------
+
+    if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
+        traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
+        //    traffic_light_pathpoint=130;
+        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
+                                                     traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
+        dSpeed = min((double)traffic_speed,dSpeed);
+        if(traffic_speed==0){
+            minDecelerate=-2.0;
+        }
+    }
+    //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------
+
+
+    //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
+
+
+    double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight,  gpsMapLine, v2xTrafficVector,
+                                                         PathPoint, secSpeed, dSpeed,  circleMode);
+
+
+    dSpeed = min(v2xTrafficSpeed,dSpeed);
+
+    //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
+
+
+    transferGpsMode2(gpsMapLine);
+
+    if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+        if(obsDistance>0 && obsDistance<8){
+                dSpeed=0;
+            }
+    }else if(obsDistance>0 && obsDistance<15){
+        dSpeed=0;
+    }
+
+    if(ServiceCarStatus.group_control){
+        dSpeed = ServiceCarStatus.group_comm_speed;
+    }
+    if(dSpeed==0){
+        if(realspeed<6)
+            minDecelerate=min(-0.5f,minDecelerate);
+        else
+            minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
+    }
+
+    gps_decition->wheel_angle = 0.0 - controlAng;
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if(acc_end<0)
+        {
+            if(minDecelerate > acc_end) minDecelerate = acc_end;
+        }
+    }
+
+    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
+
+    Point2D now_s_d=iv::decition::cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+
+    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d,now_s: %f,now_d: %f",
+            vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2,now_s_d.s,now_s_d.d);
+
+    if(ServiceCarStatus.txt_log_on==true){
+        QDateTime dt = QDateTime::currentDateTime();
+        //将数据写入文件开始
+        ofstream outfile;
+        outfile.open("control_log.txt", ostream::app);
+        outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
+              <<vehState<<'\t'<<PathPoint<<'\t'<<setprecision(4)<<dSpeed<<'\t'<<obsDistance<<'\t'<<obsSpeed<<'\t'<<realspeed<<'\t'<<minDecelerate<<'\t'
+             <<gps_decition->torque<<'\t'<<gps_decition->brake<<'\t'<<gps_decition->wheel_angle<<'\t'<<now_gps_ins.gps_x<<'\t'<<now_gps_ins.gps_y<<'\t'<<now_s_d.s<<'\t'<<now_s_d.d<<endl;
+        outfile.close();
+        //将数据写入文件结束
+    }
+
+
+    lastAngle=gps_decition->wheel_angle;
+    lastVehState=vehState;
+
+    //   qDebug("decide1 time is %d",xTime.elapsed());
+    return gps_decition;
+}
+
+
+
+void iv::decition::DecideGps00::initMethods(){
+
+    pid_Controller= new PIDController();
+    ge3_Adapter = new Ge3Adapter();
+    qingyuan_Adapter = new QingYuanAdapter();
+    vv7_Adapter = new VV7Adapter();
+    zhongche_Adapter = new ZhongcheAdapter();
+    bus_Adapter = new BusAdapter();
+    hapo_Adapter = new HapoAdapter();
+    sightseeing_Adapter = new SightseeingAdapter();
+
+
+    mpid_Controller.reset(pid_Controller);
+    mge3_Adapter.reset(ge3_Adapter);
+    mqingyuan_Adapter.reset(qingyuan_Adapter);
+    mvv7_Adapter.reset(vv7_Adapter);
+    mzhongche_Adapter.reset(zhongche_Adapter);
+    mbus_Adapter.reset(bus_Adapter);
+    mhapo_Adapter.reset(hapo_Adapter);
+    msightseeing_Adapter.reset(sightseeing_Adapter);
+
+    frenetPlanner = new FrenetPlanner();
+    //    laneChangePlanner = new LaneChangePlanner();
+
+    mfrenetPlanner.reset(frenetPlanner);
+    //    mlaneChangePlanner.reset(laneChangePlanner);
+
+}
+
+
+void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
+
+    pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if((decition->accelerator > minDecelerate) && (minDecelerate < 0))
+        {
+            decition->accelerator = minDecelerate;
+        }
+    }
+
+    if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+        ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
+        qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
+        vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
+        zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+        bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+        hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="sightseeing"){
+        sightseeing_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
+}
+
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+
+    traceOriLeft.clear();
+    traceOriRight.clear();
+
+    if (gpsMapLine.size() > 800 && PathPoint >= 0) {
+        int aimIndex;
+        if(circleMode){
+            aimIndex=PathPoint+800;
+        }else{
+            aimIndex=min((PathPoint+800),(int)gpsMapLine.size());
+        }
+
+        for (int i = PathPoint; i < aimIndex; i++)
+        {
+            int index = i % gpsMapLine.size();
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
+            pt.x += offset_real * 0.032;
+            pt.v1 = (*gpsMapLine[index]).speed_mode;
+            pt.v2 = (*gpsMapLine[index]).mode2;
+            pt.roadMode=(*gpsMapLine[index]).roadMode;
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            //csvv7
+            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
+            {
+                readyParkMode = true;
+                DecideGps00::finishPointNum = index;
+            }
+            switch (pt.v1)
+            {
+            case 0:
+                pt.speed = 80;
+                break;
+            case 1:
+                pt.speed = 25;
+                break;
+            case 2:
+                pt.speed =25;
+                break;
+            case 3:
+                pt.speed = 20;
+                break;
+            case 4:
+                pt.speed =18;
+                break;
+            case 5:
+                pt.speed = 18;
+                break;
+            case 7:
+                pt.speed = 10;
+                break;
+            case 22:
+                pt.speed = 5;
+                break;
+            case 23:
+                pt.speed = 5;
+                break;
+            default:
+                break;
+            }
+            trace.push_back(pt);
+
+
+            double hdg=90 - (*gpsMapLine[index]).ins_heading_angle;
+            if(hdg < 0)  hdg = hdg + 360;
+            if(hdg >= 360) hdg = hdg - 360;
+
+            double xyhdg  = hdg/180.0*PI;
+            double ptLeft_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg + PI / 2);
+            double ptLeft_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg + PI / 2);
+            Point2D ptLeft = Coordinate_Transfer(ptLeft_x, ptLeft_y, now_gps_ins);
+
+            double ptRight_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg - PI / 2);
+            double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
+            Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
+
+            traceOriLeft.push_back(ptLeft);
+            traceOriRight.push_back(ptRight);
+
+        }
+    }
+    return trace;
+}
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceAvoid(iv::GPS_INS now_gps_ins, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+
+    int index = -1;
+    float minDis = 10;
+
+    for (unsigned int i = 0; i < gpsTrace.size(); i++)
+    {
+        double tmpdis = sqrt((now_gps_ins.gps_x - gpsTrace[i].x) * (now_gps_ins.gps_x - gpsTrace[i].x) + (now_gps_ins.gps_y - gpsTrace[i].y) * (now_gps_ins.gps_y - gpsTrace[i].y));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis = tmpdis;
+        }
+    }
+
+    trace.clear();
+    if (index >= 0) {
+        for (unsigned int i = index; i < gpsTrace.size(); i++)
+        {
+            Point2D pt = Coordinate_Transfer(gpsTrace[i].x, gpsTrace[i].y, now_gps_ins);
+            trace.push_back(pt);
+        }
+    }
+    return trace;
+}
+
+
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+
+
+            trace.push_back(ptRight);
+        }
+    }
+
+    return trace;
+}
+
+void  iv::decition::DecideGps00::getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace) {
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceNowLeft.push_back(ptLeft);
+        gpsTraceNowRight.push_back(ptRight);
+    }
+
+}
+
+
+
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+
+
+            trace.push_back(ptRight);
+        }
+    }
+
+
+    bool use_new_method = true;
+    if  (use_new_method)
+    {
+        const int val = 300;
+        if(trace.size()>val)
+        {
+            double V = trace[300].y;
+            for (int j = 0; j < val; j++)
+            {
+                    double t = (double)j / val;
+                    double s = t*t*(3.-2.*t);
+                    double ox = s;
+                    double oy = t *( V-3.0)+3.0;
+
+                    trace[j].x=ox*trace[j].x;
+                    trace[j].y=oy;
+            }
+        }
+    }
+
+    traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+                GPS_INS gpsxy = Coordinate_UnTransfer(gpsTrace[j].x,gpsTrace[j].y,nowgps);
+                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+                traceXY.push_back(gpsinxy);
+    }
+    for(int j=0;j<trace.size();j++)
+    {
+                GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
+                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+                traceXY.push_back(gpsinxy);
+    }
+    return traceXY;
+}
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow) {
+
+
+    std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
+
+    if (offset==0)
+    {
+        trace.assign(gpsTrace.begin(), gpsTrace.end());
+    }
+    else
+    {
+        for (int j = 0; j < gpsTrace.size(); j++)
+        {
+            double sumx1 = 0, sumy1 = 0, count1 = 0;
+            double sumx2 = 0, sumy2 = 0, count2 = 0;
+            for (int k = max(0, j - 4); k <= j; k++)
+            {
+                count1 = count1 + 1;
+                sumx1 += gpsTrace[k].x;
+                sumy1 += gpsTrace[k].y;
+            }
+            for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+            {
+                count2 = count2 + 1;
+                sumx2 += gpsTrace[k].x;
+                sumy2 += gpsTrace[k].y;
+            }
+            sumx1 /= count1; sumy1 /= count1;
+            sumx2 /= count2; sumy2 /= count2;
+
+            double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+            double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+            double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+            double avoidLenth = abs(offset);
+            if (offset<0)
+            {
+                Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                               carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+                ptLeft.speed = gpsTrace[j].speed;
+                ptLeft.v1 = gpsTrace[j].v1;
+                ptLeft.v2 = gpsTrace[j].v2;
+                trace.push_back(ptLeft);
+            }
+            else
+            {
+                Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                                carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+                ptRight.speed = gpsTrace[j].speed;
+                ptRight.v1 = gpsTrace[j].v1;
+                ptRight.v2 = gpsTrace[j].v2;
+
+
+                trace.push_back(ptRight);
+            }
+        }
+    }
+
+
+    bool use_new_method = true;
+    if  (use_new_method)
+    {
+        const int val = 300;
+        if(trace.size()>val)
+        {
+            double V = trace[300].y;
+            for (int j = 0; j < val; j++)
+            {
+                    double t = (double)j / val;
+                    double s = t*t*(3.-2.*t);
+                    double ox = s;
+                    double oy = t *( V-3.0)+3.0;
+
+                    trace[j].x=ox*trace[j].x;
+                    trace[j].y=oy;
+            }
+        }
+    }
+
+    traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+                GPS_INS gpsxy = Coordinate_UnTransfer(gpsTraceNowNow[j].x,gpsTraceNowNow[j].y,nowgps);
+                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+                traceXY.push_back(gpsinxy);
+    }
+    for(int j=0;j<trace.size();j++)
+    {
+                GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
+                Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+                traceXY.push_back(gpsinxy);
+    }
+    return traceXY;
+}
+
+
+
+double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
+    double angle=0;
+    if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)
+    {
+        //   angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
+        pid_Controller->getControlDecition(gpsIns, gpsTrace,  -1,  -1,  -1, true, false, &decition);
+        angle= decition->wheel_angle;
+    }
+    return angle;
+}
+
+double iv::decition::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
+    double speed=0;
+    int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
+    speed = gpsTrace[speedPoint].speed;
+    for (int i = 0; i < speedPoint; i++) {
+        speed = min(speed, gpsTrace[i].speed);
+    }
+    return speed;
+}
+
+
+//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
+//
+//	if (!obsRadars.empty())
+//	{
+//		esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
+//
+//		if (esrIndex != -1)
+//		{
+//			 esrDistance = obsRadars[esrIndex].nomal_y;
+//
+//
+//
+//			obsSpeed = obsRadars[esrIndex].speed_y;
+//
+//		}
+//		else {
+//			esrDistance = -1;
+//		}
+//
+//	}
+//	else
+//	{
+//		esrIndex = -1;
+//		esrDistance = -1;
+//	}
+//	if (esrDistance < 0) {
+//		ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
+//	}
+//	else {
+//		ODS("\n------------------>ESR障碍物距离:%f   ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
+//	}
+//
+//	ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
+//}
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
+
+
+    int esrPathpoint;
+
+    esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);
+
+    if (esrIndex != -1)
+    {
+        //优化
+        //        double distance = 0.0;
+        //        for(int i=0; i<esrPathpoint; ++i){
+        //            distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
+        //        }
+        //        esrDistance = distance - Esr_Y_Offset;
+        //        if(esrDistance<=0){
+        //            esrDistance=1;
+        //        }
+
+
+        esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistanceAvoid() {
+
+    esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
+
+    if (esrIndexAvoid != -1)
+    {
+        esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+
+    }
+    else {
+        esrDistanceAvoid = -1;
+    }
+
+    if (esrDistanceAvoid < 0) {
+        std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
+    }
+    else {
+        std::cout << "\nESR障碍物距离Avoid:%f   %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
+    }
+
+    std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
+}
+
+
+
+
+double iv::decition::DecideGps00::limitAngle(double speed, double angle) {
+    double preAngle = angle;
+
+    if (speed > 15)
+    {
+        if (preAngle > 350)
+        {
+            preAngle = 350;
+        }
+        if (preAngle < -350)
+        {
+            preAngle = -350;
+        }
+    }
+    if (speed > 22)
+    {
+        if (preAngle > 200)
+        {
+            preAngle = 200;
+        }
+        if (preAngle < -200)
+        {
+            preAngle = -200;
+        }
+    }
+    if (speed > 25)
+    {
+        if (preAngle > 150)
+        {
+            preAngle = 150;
+        }
+        if (preAngle < -150)
+        {
+            preAngle = -150;
+        }
+    }
+    if (speed > 30)
+    {
+        if (preAngle > 70)
+        {
+            preAngle = 70;
+        }
+        if (preAngle < -70)
+        {
+            preAngle = -70;
+        }
+    }
+    if (speed > 45)                     //20
+    {
+        if (preAngle > 15)
+        {
+            preAngle = 15;
+        }
+        if (preAngle < -15)
+        {
+            preAngle = -15;
+        }
+    }
+    return preAngle;
+}
+
+
+
+double iv::decition::DecideGps00::limitSpeed(double angle, double speed) {
+
+    if (abs(angle) > 500 && speed > 8) speed = 8;
+    else if (abs(angle) > 350 && speed > 14) speed = 14;
+    else if (abs(angle) > 200 && speed > 21) speed = 21;
+    else if (abs(angle) > 150 && speed > 24) speed = 24;
+    else if (abs(angle) > 60 && speed > 29) speed = 29;
+    else if (abs(angle) > 20 && speed > 34) speed = 34;
+    return max(0.0, speed);
+}
+
+
+bool iv::decition::DecideGps00::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {
+
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
+            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
+    {
+        return false;
+    }
+
+    if (roadNum-roadNow>1)
+    {
+        for (int i = roadNow+1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow-roadNum>1)
+    {
+        for (int i = roadNow-1; i >roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+
+bool iv::decition::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
+    //lsn
+    if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
+    {
+        return false;
+    }
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
+            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
+    {
+        return false;
+    }
+
+
+    if (roadNum - roadNow>1)
+    {
+        for (int i = roadNow + 1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow - roadNum>1)
+    {
+        for (int i = roadNow - 1; i >roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+void iv::decition::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
+
+
+
+    if (lidarGridPtr == NULL)
+    {
+        iv::decition::DecideGps00::lidarDistanceAvoid = iv::decition::DecideGps00::lastLidarDisAvoid;
+    }
+    else {
+        obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
+        iv::decition::DecideGps00::lastLidarDisAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+    }
+    std::cout << "\nLidarAvoid距离:%f\n" << iv::decition::DecideGps00::lidarDistanceAvoid << std::endl;
+    getEsrObsDistanceAvoid();
+
+    //lidarDistanceAvoid = -1;   //20200103 apollo_fu
+
+    if (esrDistanceAvoid>0 && iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        if (iv::decition::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
+        {
+            obsDistanceAvoid = esrDistanceAvoid;
+            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+        else
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
+            std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+    }
+    else if (esrDistanceAvoid>0)
+    {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+    }
+    else if (iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+        obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+        std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+    }
+    else {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = 0 - secSpeed;
+    }
+
+    std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
+
+
+
+
+
+    if (iv::decition::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
+    {
+        iv::decition::DecideGps00::obsDistanceAvoid = iv::decition::DecideGps00::lastDistanceAvoid;
+        obsLostTimeAvoid++;
+    }
+    else
+    {
+        obsLostTimeAvoid = 0;
+        iv::decition::DecideGps00::lastDistanceAvoid = -1;
+    }
+
+
+
+
+    if (obsDistanceAvoid>0)
+    {
+        iv::decition::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
+    }
+
+    std::cout << "\nODSAvoid距离:%f\n" << iv::decition::DecideGps00::obsDistanceAvoid << std::endl;
+}
+
+void iv::decition::DecideGps00::init() {
+    for (int i = 0; i < roadSum; i++)
+    {
+        lastEsrIdVector.push_back(-1);
+        lastEsrCountVector.push_back(0);
+        GPS_INS gps_ins;
+        gps_ins.gps_x = 0;
+        gps_ins.gps_y = 0;
+        startAvoidGpsInsVector.push_back(gps_ins);
+        avoidMinDistanceVector.push_back(0);
+    }
+}
+
+
+
+#include <QTime>
+
+void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+    obsSd= obsPoint.obs_speed_y;
+
+    if(ServiceCarStatus.useLidarPerPredict){
+       double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
+       if (preDis<obs){
+           obs = preDis;
+           if(abs(obs-preDis>0.5)){
+            obsSd = 0-realspeed;
+           }
+       }
+    }
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+    if(obsDistance<500&&obsDistance>0){
+     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
+//void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+//                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+
+        obsCarCoordinateX=obsPoint.x;
+        obsCarCoordinateY=obsPoint.y;
+        obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+        obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+        givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+
+    obsSd= obsPoint.obs_speed_y;
+
+    if(ServiceCarStatus.useLidarPerPredict){
+       double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
+       if (preDis<obs){
+           obs = preDis;
+           if(abs(obs-preDis>0.5)){
+            obsSd = 0-realspeed;
+           }
+       }
+    }
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+    if(obsPoint.obs_type==2){
+        obsSpeed=-secSpeed;
+    }
+
+    if(obsDistance<500&&obsDistance>0){
+     std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
+
+//1220
+void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                     const std::vector<GPSData> gpsMapLine) {
+
+    double obs,obsSd;
+
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
+        }
+        if(abs(obsPoint.y)>lidarXiuZheng)
+            lidarDistance = abs(obsPoint.y)-lidarXiuZheng;   //激光距离推到车头 1220
+
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    if(lidarDistance==500){
+        lidarDistance=-1;
+    }
+
+
+
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    obs=lidarDistance;
+    //	obsSpeed = 0 - secSpeed;
+    obsSd = 0 -secSpeed;
+
+
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+    if (obsDistance <0 && obsLostTime<4)
+    {
+        obsDistance = lastDistance;
+        obsLostTime++;
+    }
+    else
+    {
+        obsLostTime = 0;
+        lastDistance = -1;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+}
+
+
+
+double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
+    double preObsDis=500;
+
+    if(!lidar_per.empty()){
+        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
+        lastPreObsDistance=preObsDis;
+
+    }else{
+        preObsDis=lastPreObsDistance;
+    }
+
+    ServiceCarStatus.mfttc = preObsDis;
+    return preObsDis;
+//    if(preObsDis<obsDistance){
+//        obsDistance=preObsDis;
+//        lastDistance=obsDistance;
+//    }
+}
+
+int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+    roadPre = -1;
+
+    for (int i =  0; i < roadSum; i++)
+    {
+        gpsTraceAvoid.clear();
+        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+
+    for(int i=0; i<roadSum;i++){
+        std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
+
+    }
+
+    checkAvoidObsTimes++;
+    if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
+    {
+        return - 1;
+    }
+
+
+    for (int i = 1; i < roadSum; i++)
+    {
+        if (roadNow + i < roadSum) {
+         //   avoidX = (roadOri-roadNow-i)*roadWidth;
+             avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
+            {
+                /*if (roadNow==roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }	*/
+                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow + i;
+                return roadPre;
+            }
+        }
+
+        if (roadNow - i >= 0)
+        {
+        //    avoidX = (roadOri - roadNow+i)*roadWidth;
+             avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+            if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
+            {
+                /*if (roadNow == roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }*/
+                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow - i;
+                return roadPre;
+            }
+        }
+
+    }
+    return roadPre;
+}
+
+int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+    roadPre = -1;
+
+    for (int i =  0; i <roadSum; i++)
+    {
+        gpsTraceBack.clear();
+        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
+        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
+    }
+
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedBackLidar = true;
+    }
+
+
+
+    checkBackObsTimes++;
+    if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
+    {
+        return -1;
+    }
+
+    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
+            (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
+    {
+        roadPre = roadOri;
+        return roadPre;
+    }
+
+    if (roadNow-roadOri>1)
+    {
+        for (int i = roadOri + 1;i < roadNow;i++) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    else if (roadNow <roadOri-1)
+    {
+        for (int i = roadOri - 1;i > roadNow;i--) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    return roadPre;
+}
+
+
+double iv::decition::DecideGps00::trumpet() {
+    if (trumpetFirstCount)
+    {
+        trumpetFirstCount = false;
+        trumpetLastTime= GetTickCount();
+        trumpetTimeSpan = 0.0;
+    }
+    else
+    {
+        trumpetStartTime= GetTickCount();
+        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
+        trumpetLastTime = trumpetStartTime;
+    }
+
+    return trumpetTimeSpan;
+}
+
+
+
+
+double iv::decition::DecideGps00::transferP() {
+    if (transferFirstCount)
+    {
+        transferFirstCount = false;
+        transferLastTime= GetTickCount();
+        transferTimeSpan = 0.0;
+    }
+    else
+    {
+        transferStartTime= GetTickCount();
+        transferTimeSpan += transferStartTime - transferLastTime;
+        transferLastTime = transferStartTime;
+    }
+
+    return transferTimeSpan;
+}
+
+
+
+void  iv::decition::DecideGps00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins) {
+
+    if (abs(now_gps_ins.speed)>0.1)
+    {
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+
+    }
+    else
+    {
+
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+        handPark = true;
+        handParkTime = duringTime;
+    }
+
+}
+
+
+
+
+void iv::decition::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins) {
+    gmapsL.clear();
+    gmapsR.clear();
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //	gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
+
+        gmapsL.push_back(gpsMapLineBeside);
+    }
+
+
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //		gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
+
+        gmapsR.push_back(gpsMapLineBeside);
+    }
+
+}
+
+
+bool iv::decition::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {
+
+    if (lidarGridPtr == NULL)
+    {
+        return false;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+
+
+
+
+        obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
+        double lidarDistance = obsPoint.y - 2.5;   //激光距离推到车头
+        //     ODS("\n超车雷达距离:%f\n", lidarDistance);
+        if (lidarDistance >-20 && lidarDistance<35)
+        {
+            checkChaoCheBackCounts = 0;
+            return false;
+        }
+        else {
+            checkChaoCheBackCounts++;
+        }
+
+        if (checkChaoCheBackCounts>2) {
+            checkChaoCheBackCounts = 0;
+            return true;
+        }
+    }
+
+    return false;
+
+}
+
+void iv::decition::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s){
+    Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);
+
+    ServiceCarStatus.group_x_local=pt.x;
+    //  ServiceCarStatus.group_y_local=pt.y;
+    ServiceCarStatus.group_y_local=s;
+    if(realspeed<0.36){
+        ServiceCarStatus.group_velx_local=0;
+        ServiceCarStatus.group_vely_local=0;
+    }else{
+        ServiceCarStatus.group_velx_local=realspeed*sin(theta)/3.6;
+        ServiceCarStatus.group_vely_local=realspeed*cos(theta)/3.6;
+    }
+
+
+    ServiceCarStatus.group_pathpoint=PathPoint;
+
+}
+
+
+
+float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                                           int pathpoint,float secSpeed,float dSpeed){
+    float traffic_speed=200;
+    float traffic_dis=0;
+    float passTime;
+    float passSpeed;
+    bool passEnable=false;
+
+    if(abs(secSpeed)<0.1){
+        secSpeed=0;
+    }
+
+
+
+    if(pathpoint <= traffic_light_pathpoint){
+        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }else{
+        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+        for(int i=0;i<traffic_light_pathpoint;i++){
+            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    //    if(traffic_light_color != 0)
+    //    {
+    //        int a = 3;
+    //    }
+
+
+    if(traffic_light_color==0 && traffic_dis<10){
+        traffic_speed=0;
+    }
+    //    else   //20200108
+    //    {
+    //        traffic_speed=10;
+    //    }
+    return  traffic_speed;
+
+    passSpeed = min((float)(dSpeed/3.6),secSpeed);
+    passTime = traffic_dis/(dSpeed/3.6);
+
+
+
+
+
+    switch(traffic_light_color){
+    case 0:
+        if(passTime>traffic_light_time+1 && traffic_dis>10){
+            passEnable=true;
+        }else{
+            passEnable=false;
+        }
+        break;
+    case 1:
+        if(passTime<traffic_light_time-1 && traffic_dis<10){
+            passEnable=true;
+        }else{
+            passEnable = false;
+        }
+        break;
+    case 2:
+        if(passTime<traffic_light_time){
+            passEnable= true;
+        }else{
+            passEnable=false;
+        }
+        break;
+
+
+    default:
+        break;
+    }
+
+    if(!passEnable){
+        if(traffic_dis<5){
+            traffic_speed=0;
+        }else if(traffic_dis<10){
+            traffic_speed=5;
+        }else if(traffic_dis<20){
+            traffic_speed=15;
+        }else if(traffic_dis<30){
+            traffic_speed=25;
+        }else if(traffic_dis<50){
+            traffic_speed=30;
+        }
+    }
+    return traffic_speed;
+
+}
+
+void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
+{
+    //    Point2D obsCombinePoint = Point2D(-1,-1);
+    iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
+    double obsSd;
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
+        iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
+        lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;
+
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    FrenetPoint esr_obs_frenet_point;
+    getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+    if(esrDistance<0){
+        esrDistance=500;
+    }
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
+    myesrDistance = esrDistance;
+
+    if(lidarDistance==500){
+        lidarDistance=-1;
+    }
+    if(esrDistance==500){
+        esrDistance=-1;
+    }
+
+    ServiceCarStatus.mRadarObs = esrDistance;
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    //    //zhuanwan pingbi haomibo
+    //    if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
+    //        esrDistance=-1;
+    //    }
+
+    if (esrDistance>0 && lidarDistance > 0)
+    {
+        if (lidarDistance >= esrDistance)
+        {
+            obs = esrDistance;
+            //            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
+            obsSd = obsSpeed;
+            //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+            //            obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obs = lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            //            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
+            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+        }
+        else
+        {
+            obs=lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            obsSd = 0 -secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
+            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
+        }
+    }
+    else if (esrDistance>0)
+    {
+        obs = esrDistance;
+        //        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+        obsSd = obsSpeed;
+        //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+    }
+    else if (lidarDistance > 0)
+    {
+        obs = lidarDistance;
+        //        obsCombinePoint = obsPoint;
+        obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+    }
+    else {
+        obs = esrDistance;
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
+    }
+
+    obsDistance=obs;
+    obsSpeed=obsSd;
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    if(obs<0){
+        obsDistance=500;
+    }else{
+        obsDistance=obs;
+    }
+}
+
+void iv::decition::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
+
+
+    esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum);
+
+    if (esrIndex != -1)
+    {
+        esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
+
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point, FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps) {
+
+    esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);
+
+    if (esrIndex != -1)
+    {
+        //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
+        //严格来说应是 esrDistance=障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
+        esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset;  //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。
+
+        double speedx=ServiceCarStatus.obs_radar[esrIndex].speed_x;  //障碍物相对于车辆x轴的速度
+        double speedy=ServiceCarStatus.obs_radar[esrIndex].speed_y;  //障碍物相对于车辆y轴的速度
+        double speed_combine = sqrt(speedx*speedx+speedy*speedy);    //将x、y轴两个方向的速度求矢量和
+        //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+        //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+        double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
+
+        obsSpeed = speed_combine*cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
+    v2xTrafficVector.clear();
+    for (int var = 0; var < gpsMapLine.size(); var++) {
+        if(gpsMapLine[var]->roadMode==6|| gpsMapLine[var]->mode2==1000001){
+            v2xTrafficVector.push_back(var);
+        }
+    }
+}
+
+float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
+                                                             int pathpoint,float secSpeed,float dSpeed, bool circleMode){
+    float trafficSpeed=200;
+    int nearTraffixPoint=-1;
+    float nearTrafficDis=0;
+    int traffic_color=0;
+    int traffic_time=0;
+    bool passThrough=false;
+    float dSecSpeed=dSpeed/3.6;
+
+    if(v2xTrafficVector.empty()){
+        return trafficSpeed;
+    }
+    if(!circleMode){
+        if(pathpoint>v2xTrafficVector.back()){
+            return trafficSpeed;
+        }else {
+            for(int i=0; i< v2xTrafficVector.size();i++){
+                if (pathpoint<= v2xTrafficVector[i]){
+                    nearTraffixPoint=v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }else if(circleMode){
+        if(pathpoint>v2xTrafficVector.back()){
+            nearTraffixPoint=v2xTrafficVector[0];
+        }else {
+            for(int i=0; i< v2xTrafficVector.size();i++){
+                if (pathpoint<= v2xTrafficVector[i]){
+                    nearTraffixPoint=v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }
+
+
+    if(nearTraffixPoint!=-1){
+        for(int i=pathpoint;i<nearTraffixPoint;i++){
+            nearTrafficDis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    if(nearTrafficDis>50){
+        return trafficSpeed;
+    }
+
+
+    int roadMode = gpsMapLine[pathpoint]->roadMode;
+    if(roadMode==14 || roadMode==16){
+        traffic_color=trafficLight.leftColor;
+        traffic_time=trafficLight.leftTime;
+    }else if(roadMode==15 ||roadMode==17){
+        traffic_color=trafficLight.rightColor;
+        traffic_time=trafficLight.rightTime;
+    }else {
+        traffic_color=trafficLight.straightColor;
+        traffic_time=trafficLight.straightTime;
+    }
+
+
+    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
+    if(passThrough){
+        return trafficSpeed;
+    }else{
+        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
+        if(nearTrafficDis<6){
+            float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
+            minDecelerate=min(minDecelerate,decelerate);
+        }
+        return trafficSpeed;
+    }
+
+    return trafficSpeed;
+}
+
+bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
+    float passTime=0;
+    if (trafficColor==2 || trafficColor==3){
+        return false;
+    }else if(trafficColor==0){
+        return true;
+    }else{
+        passTime=trafficDis/dSecSpeed;
+        if(passTime+1< trafficTime){
+            return true;
+        }else{
+            return false;
+        }
+
+    }
+
+}
+
+float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
+    float limit=200;
+    if(trafficDis<10){
+        limit = 0;
+    }else if(trafficDis<15){
+        limit = 5;
+    }else if(trafficDis<20){
+        limit=10;
+    }else if(trafficDis<30){
+        limit=15;
+    }
+    return limit;
+}
+
+void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
+    static int obstacle_disable=0;
+    static int speed_slowdown_flag=0;
+    static bool lock_flag=false;
+    double forecast_distance=0;
+    int forecast_point_num=0;
+    bool cross=false;
+
+    double secLowSpeed=ServiceCarStatus.mroadmode_vel.mfmode18/3.6;   //m/s
+    if(secSpeed>secLowSpeed)
+    {
+        forecast_distance=secSpeed*secSpeed-secLowSpeed*secLowSpeed+5;
+        forecast_point_num=((int)forecast_distance)*10;
+        if((PathPoint+forecast_point_num+2)>gpsMapLine.size())
+                                                forecast_point_num=0;
+    }
+    if((PathPoint+forecast_point_num-8>0)&&(PathPoint+forecast_point_num+8<gpsMapLine.size()))
+    {
+        for(int i=PathPoint+forecast_point_num-8;i<PathPoint+forecast_point_num+8;i++)
+        {
+            if(gpsMapLine[i]->mode2==5000)
+                cross=true;
+        }
+    }
+
+    //givlog->debug("decition_brain","PATHFORE: %d,Forecast: %d,cross: %d",
+                //PathPoint+forecast_point_num,forecast_point_num,cross);
+    if(  gpsMapLine[PathPoint]->mode2==3000){
+        if(obsDistance>4){       //7   zj-5
+            obsDistance=200;
+        }else{
+            lock_flag=false;
+            obsSpeed=-realspeed/3.6;
+        }
+        if((realspeed>3)&&(lock_flag==false)){
+            minDecelerate=-0.5;
+        }else{
+            dSpeed=min(dSpeed,3.0);
+            lock_flag=true;
+        }
+    }
+    else if(gpsMapLine[PathPoint]->mode2==3001){
+             obstacle_disable=1;
+    }else if(gpsMapLine[PathPoint]->mode2==3002){
+             obstacle_disable=0;
+    }else if(gpsMapLine[PathPoint]->mode2==4000){
+        //ServiceCarStatus.msysparam.vehWidth=5.6;
+    }else if(cross==true){
+            speed_slowdown_flag=1;
+            lock_flag=false;
+    }else if(gpsMapLine[PathPoint]->mode2==5001){
+            speed_slowdown_flag=0;
+    }
+
+    if(obstacle_disable==1){
+            obsDistance=200;
+    }
+
+
+    if(speed_slowdown_flag==1)
+    {
+            if((realspeed>ServiceCarStatus.mroadmode_vel.mfmode18)&&(lock_flag==false)){
+                minDecelerate=-0.3;
+            }else{
+                dSpeed=min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
+                lock_flag=true;
+            }
+    }
+}
+
+float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){
+    if(roadAim==roadOri){
+        return 0;
+    }
+    float x=0;
+    float veh_to_roadSide=(gps->mfLaneWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
+    float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
+    if(!ServiceCarStatus.inRoadAvoid){
+        x= (roadOri-roadAim)*gps->mfLaneWidth;
+    }else{
+        int num=roadOri-roadAim;
+        switch (abs(num)%3) {
+        case 0:
+            x=(num/3)*gps->mfLaneWidth;
+            break;
+        case 1:
+            if(num>0){
+                x=(num/3)*gps->mfLaneWidth +veh_to_roadSide;
+            }else{
+                x=(num/3)*gps->mfLaneWidth -veh_to_roadSide;
+            }
+            break;
+        case 2:
+            if(num>0){
+                x=(num/3)*gps->mfLaneWidth +veh_to_roadSide+roadSide_to_roadSide;
+            }else{
+                x=(num/3)*gps->mfLaneWidth -veh_to_roadSide-roadSide_to_roadSide;
+            }
+
+            break;
+        default:
+            break;
+        }
+
+    }
+    return x;
+}
+
+double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint){
+    double dist_to_end=0;
+    for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
+        if(gpsMapLine[i]->mode2!=23)
+            dist_to_end+=GetDistance(*gpsMapLine[i+1], *gpsMapLine[i]);
+    }
+    return dist_to_end;
+}
+
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft, int* avoidXRight ){
+
+    double RightBoundary=(((double)roadOriginal+0.5)*roadWidthEvery-vehicleWidth/2);
+    *avoidXRight=((int)RightBoundary);
+    double LeftBoundary=(((double)(roadSumMap-roadOriginal-1)+0.5)*roadWidthEvery-vehicleWidth/2);
+    *avoidXLeft=(-(int)LeftBoundary);
+}
+
+int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int offsetLast) {
+
+    int signed_record_avoidX=offsetLast,record_avoidX=20;
+    double obs_cur_distance=500,record_obstacle_distance;
+    iv::Point2D obs_spline_record;
+
+    obs_property.clear();
+    for (int i =  avoidLeft; i <= avoidRight; i++)
+    {
+        obsvalue x_value;
+        obsvalue *x=&x_value;
+        x_value.avoid_distance=i;
+        gpsTraceAvoid.clear();
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
+        obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,lidar_per,x);
+        //computeObsOnRoadNew(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per,x);
+        obs_property.push_back(x_value);
+        if(i==offsetLast){
+            obs_cur_distance=x_value.obs_distance;
+            record_obstacle_distance=obs_cur_distance;
+        }
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+        obsSpline.push_back(obs_spline_record);
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+    if(offsetLast==0)
+    {
+              unsigned int vector_num_record=0;
+              for(int j=0;j<obs_property.size();j++){
+                    if(obs_property[j].obs_distance>100){
+                            if(abs(obs_property[j].avoid_distance)<record_avoidX){
+                                    record_avoidX=abs(obs_property[j].avoid_distance);
+                                    signed_record_avoidX=obs_property[j].avoid_distance;
+                                    vector_num_record=j;
+                            }
+                    }
+               }
+               if((signed_record_avoidX<0)&&(signed_record_avoidX-1>=avoidLeft))
+               {
+                      //int obs_test=abs(signed_record_avoidX)-1;
+                      int obs_test=vector_num_record-1;
+                      if(obs_property[obs_test].obs_distance>100){
+                                signed_record_avoidX-=1;
+                                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+                      }
+               }
+               if(signed_record_avoidX==0){
+                   for(int j=0;j<obs_property.size();j++){
+                       if(obs_property[j].obs_distance>obs_cur_distance+15){
+                           if(obs_property[j].obs_distance>record_obstacle_distance){
+                                   record_avoidX=abs(obs_property[j].avoid_distance);
+                                   signed_record_avoidX=obs_property[j].avoid_distance;
+                                   record_obstacle_distance=obs_property[j].obs_distance;
+                           }
+                       }
+                   }
+               }
+    }else{
+                for(int j=0;j<obs_property.size();j++){
+                    if(obs_property[j].obs_distance>100){
+                            if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast)){
+                                    record_avoidX=abs(obs_property[j].avoid_distance);
+                                    signed_record_avoidX=obs_property[j].avoid_distance;
+                            }
+
+                    }
+                }
+                if(signed_record_avoidX==0){
+                            return 0;
+                }else if(obs_cur_distance>30){
+                            signed_record_avoidX=offsetLast;
+                }else if((obs_cur_distance<=30)&&(signed_record_avoidX==offsetLast)){
+                            for(int j=0;j<obs_property.size();j++){
+                                if(obs_property[j].obs_distance>obs_cur_distance+15){
+                                    if(obs_property[j].obs_distance>record_obstacle_distance){
+                                            record_avoidX=abs(obs_property[j].avoid_distance);
+                                            signed_record_avoidX=obs_property[j].avoid_distance;
+                                            record_obstacle_distance=obs_property[j].obs_distance;
+                                    }
+                                }
+                            }
+                }
+    }
+//   if(front_car_id>0){
+//           if((obs_cur_distance<50)&&(front_car.front_car_dis<30)&&(front_car_decide_avoid==true)){
+//                    signed_record_avoidX=front_car.avoidX;
+//                    return signed_record_avoidX;
+//           }
+
+//           if((obs_cur_distance<50)&&(front_car.front_car_dis<150)&&(front_car.avoidX!=0||front_car.vehState!=0)){
+//                    signed_record_avoidX=front_car.avoidX;
+//                    return signed_record_avoidX;
+//           }
+//   }
+
+    if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
+    {
+
+                        avoidMinDistance= obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+                        startAvoidGps=now_gps_ins;
+                        return signed_record_avoidX;
+    }
+    return offsetLast;
+}
+
+void iv::decition::DecideGps00::computeObsOnRoadNew(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per,obsvalue* obsva) {
+
+    double obs=-1,obsSd=-1;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+    obsSd= obsPoint.obs_speed_y;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    if(obs<0){
+        obsva->obs_distance=500;
+    }else{
+        obsva->obs_distance=obs;
+        obsva->obs_speed=obsSd;
+    }
+}
+
+iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,std::vector<iv::Perception::PerceptionOutput> lidar_per,obsvalue* obsva) {
+
+    double obs=-1,obsSd=-1;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
+
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        obsCarCoordinateDistance=obsPoint.y;
+
+        if((obsCarCoordinateDistance>0)&&(obsCarCoordinateDistance<100))
+        {
+            obsCarCoordinateX=obsPoint.x;
+            obsCarCoordinateY=obsPoint.y;
+            obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+            obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+            iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+            givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+            obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+            obsFrenet.d=roadNum;
+
+            //test
+            //obsFrenet.s=obsPoint.y;
+            //obsFrenet.d=roadNum;
+
+        }else{
+            obsFrenet.s=500;
+            obsFrenet.d=roadNum;
+        }
+
+        if (obsFrenet.s<0)
+        {
+            obsFrenet.s=0;
+        }
+        lidarDistance = obsFrenet.s;
+
+        lastLidarDis = lidarDistance;
+    }
+
+    obs = lidarDistance;
+    obsSd= obsPoint.obs_speed_y;
+    obsva->obs_distance=obs;
+    obsva->obs_speed=obsSd;
+
+    return obsFrenet;
+}
+
+int iv::decition::DecideGps00::computeBackDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int avoidXNow) {
+
+    obs_property.clear();
+    double record_avoidX=20,signed_record_avoidX=0,obs_cur_distance=500,obs_cur_speed=0;
+    for (int i =  avoidLeft; i <= avoidRight; i++)
+    {
+        obsvalue x_value;
+        obsvalue *x=&x_value;
+        x_value.avoid_distance=i;
+        gpsTraceBack.clear();
+        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, i);
+        computeObsOnRoadNew(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per,x);
+        obs_property.push_back(x_value);
+        if(i==0){
+            obs_cur_distance=x_value.obs_distance;
+            obs_cur_speed=x_value.obs_speed;
+        }
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+
+//    if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
+//                        return 0;
+    if(front_car_id>0)
+    {
+        if((front_car.avoidX==0)&&(front_car.vehState==0)&&(front_car.front_car_dis<150)&&(vehState==normalRun)&&(obs_cur_distance>15))
+        {
+            if(PathPoint+300<gpsMapLine.size()){
+                for(int k=PathPoint;k<PathPoint+300;k++){
+                        if((gpsMapLine[k]->mfCurvature>0.02))
+                                         return 20;
+                }
+                return 0;
+            }
+        }
+    }
+    /*if((obs_cur_distance>100)&&(GetDistance(now_gps_ins, startAvoidGps)>max(avoidMinDistance,40.0)))
+    {
+        if(PathPoint+300<gpsMapLine.size()){
+            for(int k=PathPoint;k<PathPoint+300;k++){
+                    if((gpsMapLine[k]->mfCurvature>0.02))
+                                     return 20;
+            }
+            return 0;
+        }
+    }*/
+    if((obs_cur_distance>100||obs_cur_speed>1)&&(lastVehState==normalRun))
+    {
+        if(PathPoint+300<gpsMapLine.size()){
+            for(int k=PathPoint;k<PathPoint+300;k++){
+                    if((gpsMapLine[k]->mfCurvature>0.02))
+                                     return 20;
+            }
+            return 0;
+        }
+    }
+
+    return 20;
+}
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/decide_gps_00.h

@@ -0,0 +1,298 @@
+#pragma once
+#ifndef _IV_DECITION__DECIDE_GPS_00_
+#define _IV_DECITION__DECIDE_GPS_00_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_planner/frenet_planner.h>
+#include<decition/adc_controller/pid_controller.h>
+#include <decition/adc_adapter/ge3_adapter.h>
+#include <decition/adc_adapter/qingyuan_adapter.h>
+#include <decition/adc_adapter/vv7_adapter.h>
+#include <decition/adc_adapter/zhongche_adapter.h>
+#include <decition/adc_adapter/bus_adapter.h>
+#include <decition/adc_adapter/hapo_adapter.h>
+#include <decition/adc_adapter/sightseeing_adapter.h>
+#include "perception/perceptionoutput.h"
+#include "ivlog.h"
+#include <memory>
+#include <common/tracepointstation.h>
+#include <decition/adc_planner/spline_planner.h>
+
+namespace iv {
+namespace decition {
+//根据传感器传输来的信息作出决策
+class DecideGps00 {
+public:
+    DecideGps00();
+    ~DecideGps00();
+
+    static double minDis;
+    static double maxAngle;
+    static int lastGpsIndex;
+    static double lidarDistance;
+    static double myesrDistance;
+    static double lastLidarDis;
+    static double lastLidarDisAvoid;
+
+    static double lastPreObsDistance;
+
+    static int gpsLineParkIndex;
+    static int gpsMapParkIndex;
+
+    static double lastDistance;
+
+    static float xiuzhengCs;
+
+    static double controlAng;
+    double laneAng;
+    static double controlSpeed;
+    static double controlBrake;
+
+    static bool parkMode;
+    static bool readyParkMode;
+
+    static bool zhucheMode;
+    static bool readyZhucheMode;
+    static bool hasZhuched;
+    static double Iv;
+    static double lastEv;
+    static double lastVt;
+    static double lastU;
+    static double obsDistance;
+    static double obsSpeed;
+    static double lidarDistanceAvoid;
+    static double obsDistanceAvoid;
+    static double lastDistanceAvoid;
+
+    GPS_INS group_ori_gps;
+    GPS_INS startAvoid_gps_ins;
+    static int finishPointNum;
+    static int zhuchePointNum;
+    double avoidRunDistance=0;
+
+    std::vector<iv::GPS_INS> startAvoidGpsInsVector;
+    std::vector<double> avoidMinDistanceVector;
+    std::vector<int> v2xTrafficVector;
+    iv::GPS_INS startAvoidGpsIns,startBackGpsIns;
+
+    double avoidMinDistance;
+    iv::GPS_INS startAvoidGps;
+
+    ///kkcai, 2020-01-03
+    //bool isFirstRun = true;
+    static bool isFirstRun;
+    /////////////////////////////////////
+
+//    static bool startingFlag;//起步标志，在起步时需要进行frenet规划。
+    static float minDecelerate;
+
+
+    double startTime = 0;
+    double firstTime = 0;
+
+    bool circleMode=false;
+
+    int avoidTimes = 0;
+    int backTimes = 0;
+
+    int checkAvoidObsTimes = 0;
+    int checkBackObsTimes = 0;
+
+    bool hasCheckedAvoidLidar=false;
+    bool hasCheckedBackLidar=false;
+
+    bool personWaited = false;
+    bool roadLightWaited = false;
+
+    double decision_Angle = 0;
+    double lastAngle=0;
+
+    iv::Point2D obsPoint;
+
+    int checkAvoidTimes=0;
+    int checkBackTimes=0;
+    int chaocheCounts=0;
+
+    static int PathPoint;
+    float lastGroupOffsetX=0.0;
+
+    GPS_INS traffic_light_gps;
+    int traffic_light_pathpoint;
+
+
+    bool changingDangWei=false;
+
+    struct obsvalue{
+        int avoid_distance;
+        double obs_distance;
+        double obs_speed;
+    };
+    std::vector<obsvalue> obs_property;
+
+    struct front{
+        GPS_INS front_car_ins;
+        int vehState;
+        int avoidX;
+        double obs_distance;
+        double obs_speed;
+        double front_car_dis;
+        bool   front_car_avoid;
+    };
+    front front_car;
+
+
+    BaseController *pid_Controller;
+    BaseAdapter *ge3_Adapter;
+    BaseAdapter *qingyuan_Adapter;
+    BaseAdapter *vv7_Adapter;
+    BaseAdapter *zhongche_Adapter;
+    BaseAdapter *bus_Adapter;
+    BaseAdapter *hapo_Adapter;
+    BaseAdapter *sightseeing_Adapter;
+
+    std::shared_ptr<BaseController> mpid_Controller;
+    std::shared_ptr<BaseAdapter> mge3_Adapter;
+    std::shared_ptr<BaseAdapter> mqingyuan_Adapter;
+    std::shared_ptr<BaseAdapter> mvv7_Adapter;
+    std::shared_ptr<BaseAdapter> mzhongche_Adapter;
+    std::shared_ptr<BaseAdapter> mbus_Adapter;
+    std::shared_ptr<BaseAdapter> mhapo_Adapter;
+    std::shared_ptr<BaseAdapter> msightseeing_Adapter;
+
+	FrenetPlanner *frenetPlanner;
+    SplinePlanner *splinePlanner;
+//    BasePlanner *laneChangePlanner;
+	
+	std::shared_ptr<FrenetPlanner> mfrenetPlanner;
+//    std::shared_ptr<BasePlanner> mlaneChangePlanner;
+
+
+    Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
+                              std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);
+
+    void initMethods();
+    static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, bool circleMode);
+    static std::vector<Point2D> getGpsTraceAvoid(GPS_INS rp, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode);
+
+
+    static double getAngle(std::vector<Point2D> gpsTrace);
+    double getAngle(std::vector<Point2D> gpsTrace, GPS_INS gpsIns,iv::decition::Decition decition);
+    static double getSpeed(std::vector<Point2D> gpsTrace);
+
+    static void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
+    static void getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
+    static void getEsrObsDistanceAvoid();
+
+    void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
+    void phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns );
+
+    double limitAngle(double speed, double angle);
+    double limitSpeed(double angle, double speed);
+
+    bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+    bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+
+    void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    void computeObsOnRoadXY(iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+
+    void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);
+    void computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr,const std::vector<Point2D>& gpsTrace, double & obs, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
+
+    double predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed);
+
+    int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins,std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+
+    void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);
+
+    void  handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins);
+
+    bool checkChaoCheBack(iv::LidarGridPtr);
+    double trumpet();
+    double transferP();
+
+    bool nearStation;
+
+    void updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s);
+    void init();
+
+    std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
+    std::vector<Point2D>  getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps);
+    std::vector<Point2D>  getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow);
+    void getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace);
+    std::vector<iv::Point2D> getGpsTraceBackExa(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int pathpo);
+
+    float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                   int pathpoint,float secSpeed,float dSpeed);
+    void getV2XTrafficPositionVector( const std::vector<GPSData> gpsMapLine);
+    float ComputeV2XTrafficLightSpeed(iv::TrafficLight TrafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
+                                   int pathpoint,float secSpeed,float dSpeed,bool circleMode);
+    float getTrafficLightSpeed(int traffic_light_color, int traffic_light_time, float dis);
+
+    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
+    float computeTrafficSpeedLimt(float trafficDis);
+
+
+    void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
+    float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);
+    double computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint);
+    void   computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft,int* avoidXRight );
+    int computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int offsetLast);
+    void computeObsOnRoadNew(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                     const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per,obsvalue* obsva);
+    int computeBackDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per,int avoidLeft,int avoidRight,int avoidXNow);
+    iv::Point2D computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                     const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,std::vector<iv::Perception::PerceptionOutput> lidar_per,obsvalue* obsva);
+
+
+    GPS_INS aim_gps_ins;
+    GPS_INS chaoche_start_gps;
+    bool is_arrivaled=false;
+
+    double chaocheObsDis = 0;
+    double preChaocheDis = 0;
+    double aimObsSpeed = 0;
+    double followSpeed = 30;
+    double chaocheSpeed = 50;
+    int checkChaoCheBackCounts = 0;
+
+    float lastTorque=0;
+    float splimit=-1;
+
+    float  ObsTimeSpan=-1;
+    double ObsTimeStart=-1;
+    double ObsTimeEnd=-1;
+    float ObsTimeWidth=70.0;   //500   zj-30
+    std::vector<iv::TracePoint> planTrace;
+
+    bool roadNowStart=true;
+private:
+    //  void changeRoad(int roadNum);
+
+
+};
+
+}
+}
+
+extern bool handPark;
+extern long handParkTime;
+extern bool rapidStop;
+extern int gpsMissCount;
+extern bool changeRoad;
+extern double avoidX;
+extern int avoidXNew;
+extern bool parkBesideRoad;
+extern double steerSpeed;
+extern bool transferPieriod;
+extern bool transferPieriod2;
+extern double traceDevi;
+extern std::vector<std::vector<double>> doubledata;
+#endif // !  _IV_DECITION__DECIDE_GPS_00_
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/decide_line_00.h

@@ -0,0 +1,252 @@
+#pragma once
+#ifndef _IV_DECITION__DECIDE_LINE_00_
+#define _IV_DECITION__DECIDE_LINE_00_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include<vector> 
+#include <decition/gnss_coordinate_convert.h>
+#include <decition/decide_gps_00.h>
+
+
+
+namespace iv {
+	namespace decition {
+		//根据传感器传输来的信息作出决策
+        class DecideLine00 {
+		public:
+            DecideLine00();
+            ~DecideLine00();
+
+			static  double minDis;
+			static double maxAngle;
+			static   int lastGpsIndex;
+			static double lidarDistance;
+            static double myesrDistance;
+			static double lastLidarDis;
+			static double lastLidarDisAvoid;
+
+			
+			
+
+			static  int gpsLineParkIndex;
+			static  int gpsMapParkIndex;
+
+
+			static double lastDistance;
+
+		
+
+			
+
+
+
+			static double controlAng;
+			static double controlSpeed;
+			static double controlBrake;
+
+			static bool parkMode;
+			static bool readyParkMode;
+
+			static bool zhucheMode;
+			static bool readyZhucheMode;
+			static bool hasZhuched;
+			static double Iv;
+			static double lastEv;
+			static double lastVt;
+			static double lastU;
+			static double obsDistance;
+			static double lidarDistanceAvoid;
+			static double obsDistanceAvoid;
+			static double lastDistanceAvoid;
+
+			GPS_INS startAvoid_gps_ins;
+			static int finishPointNum;
+			static int zhuchePointNum;
+			double avoidRunDistance=0;
+
+			std::vector<iv::GPS_INS> startAvoidGpsInsVector;
+			std::vector<double> avoidMinDistanceVector;
+
+
+			bool isFirstRun = true;
+			double startTime = 0;
+			double firstTime = 0;
+
+			int avoidTimes = 0;
+			int backTimes = 0;
+
+			int checkAvoidObsTimes = 0;
+			int checkBackObsTimes = 0;
+
+			bool hasCheckedAvoidLidar=false;
+			bool hasCheckedBackLidar=false;
+
+			bool personWaited = false;
+			bool roadLightWaited = false;
+
+			double decision_Angle = 0;
+            double lastAngle=0;
+
+			iv::Point2D obsPoint;
+
+			int checkAvoidTimes=0;
+			int checkBackTimes=0;
+
+			 static int PathPoint;
+//             float dSpeed=0;
+//             float dSecSpeed=0;
+
+			//		Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, std::vector<ObstacleBasic> obsRadars, iv::LidarGridPtr lidarGridPtr);
+
+            Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+
+			static std::vector<Point2D> getGpsTrace(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex);
+             std::vector<Point2D> getGpsTraceByMobileEye(float a,float b, float c);
+		
+
+			static double getAngle(std::vector<Point2D> gpsTrace);
+			static double getSpeed(std::vector<Point2D> gpsTrace);
+
+			//		static void getEsrObs(std::vector<iv::ObstacleBasic> obsRadars);
+			static void getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum);
+			static void getEsrObsDistanceAvoid();
+
+			void getObsAvoid(iv::LidarGridPtr lidarGridPtr);
+
+            void phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDistance, double obsSpeed, GPS_INS gpsIns );
+
+			double limitAngle(double speed, double angle);
+			double limitSpeed(double angle, double speed);
+
+			bool checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+			bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
+
+			void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum);
+
+			int chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins);
+			int chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine);
+			
+
+			void getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins);
+
+			void  handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins);
+
+
+			double trumpet();
+
+			void init();
+
+			std::vector<Point2D>  getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset);
+
+			GPS_INS aim_gps_ins;
+			bool is_arrivaled=false;
+
+
+
+
+
+            double offset_real = 0;
+            double realspeed;
+            double dSpeed;
+            double dSecSpeed;
+            double secSpeed;
+            double ac;
+
+
+
+
+
+
+            int esrLineIndex = -1;
+            int esrIndexAvoid = -1;
+            double obsSpeed = 0;
+            double obsSpeedAvoid = 0;
+
+            double esrLineDistance = -1;
+            double esrDistanceAvoid = -1;
+            int obsLostTime = 0;
+            int obsLostTimeAvoid = 0;
+
+
+
+
+            // double avoidTime = 0;
+
+
+            double avoidX =0;
+            float roadWidth = 4;
+            int roadSum =4;
+            int roadNow = 3;
+            int roadOri =3;
+            int roadPre = -1;
+            int lastRoadOri = 3;
+
+            int lightTimes = 0;
+
+
+            int lastRoadNum=1;
+
+            int stopCount = 0;
+
+            int gpsMissCount = 0;
+
+            bool rapidStop = false;
+
+            bool hasTrumpeted = false;
+
+
+            bool changingDangWei=false;
+
+            //实验手刹
+            bool handFirst = true;
+            double handTimeSpan = 0;
+            double handStartTime = 0;
+            double handLastTime = 0;
+            bool handPark = false;
+            long handParkTime=10000;
+
+            //喇叭
+            bool trumpetFirstCount=true;
+            double trumpetTimeSpan = 0;
+            double trumpetStartTime = 0;
+            double trumpetLastTime = 0;
+
+            bool busMode = false;
+
+            enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
+                waitAvoid ,shoushaTest,justRun} vehState;
+
+
+
+            std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri;
+
+
+            float  outGarageDistance=0;
+            double outGarageTimeLast=-1;
+            double outGarageTimeNow=-1;
+            float outGarageLong=10;
+
+            double waitGpsTimeStart=-1;
+            double waitGpsTimeNow=-1;
+            float waitGpsTimeSpan=-1;
+            float waitGpsTimeWidth=5000;
+            bool waitGps=false;
+
+
+		private:
+               iv::decition::DecideGps00* decitionGpstool ;			//决策器
+               std::shared_ptr<DecideGps00> mdecitionGpstool;
+		};
+
+	}
+}
+
+extern bool handPark;
+extern long handParkTime;
+extern bool rapidStop;
+extern int gpsMissCount;
+#endif // !  _IV_DECITION__DECIDE_LINE_00_
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/decide_line_00_.cpp

@@ -0,0 +1,979 @@
+#include <decition/decide_line_00.h>
+#include <decition/compute_00.h>
+#include <decition/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/transfer.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+#include <time.h>
+using namespace std;
+
+#define PI (3.1415926535897932384626433832795)
+
+iv::decition::DecideLine00::DecideLine00() {
+      decitionGpstool = new iv::decition::DecideGps00();
+      mdecitionGpstool.reset(decitionGpstool);
+}
+iv::decition::DecideLine00::~DecideLine00() {
+
+}
+
+
+int iv::decition::DecideLine00::PathPoint = -1;
+double iv::decition::DecideLine00::minDis = iv::MaxValue;
+double iv::decition::DecideLine00::maxAngle = iv::MinValue;
+//int iv::decition::DecideLine00::lastGpsIndex = iv::MaxValue;
+int iv::decition::DecideLine00::lastGpsIndex = 0;
+double iv::decition::DecideLine00::controlSpeed = 0;
+double iv::decition::DecideLine00::controlBrake = 0;
+double iv::decition::DecideLine00::controlAng = 0;
+double iv::decition::DecideLine00::Iv = 0;
+double iv::decition::DecideLine00::lastU = 0;
+double iv::decition::DecideLine00::lastVt = 0;
+double iv::decition::DecideLine00::lastEv = 0;
+
+int iv::decition::DecideLine00::gpsLineParkIndex = 0;
+int iv::decition::DecideLine00::gpsMapParkIndex = 0;
+double iv::decition::DecideLine00::lastDistance = MaxValue;
+double iv::decition::DecideLine00::obsDistance = -1;
+double iv::decition::DecideLine00::obsDistanceAvoid = -1;
+double iv::decition::DecideLine00::lastDistanceAvoid = -1;
+
+double iv::decition::DecideLine00::lidarDistance = -1;
+double iv::decition::DecideLine00::myesrDistance = -1;
+double iv::decition::DecideLine00::lastLidarDis = -1;
+
+bool iv::decition::DecideLine00::parkMode = false;
+bool iv::decition::DecideLine00::readyParkMode = false;
+
+bool iv::decition::DecideLine00::zhucheMode = false;
+bool iv::decition::DecideLine00::readyZhucheMode = false;
+bool iv::decition::DecideLine00::hasZhuched = false;
+
+double iv::decition::DecideLine00::lastLidarDisAvoid = -1;
+double iv::decition::DecideLine00::lidarDistanceAvoid = -1;
+
+int iv::decition::DecideLine00::finishPointNum = 0;
+int iv::decition::DecideLine00::zhuchePointNum = 0;
+
+
+
+
+
+//日常展示
+
+iv::decition::Decition    iv::decition::DecideLine00::getDecideFromGPS(GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, iv::LidarGridPtr lidarGridPtr,
+                                                                       std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+	Decition gps_decition(new DecitionBasic);
+
+
+	if (isFirstRun)
+	{
+		vehState = normalRun;	
+		startAvoid_gps_ins = now_gps_ins;
+
+
+		ServiceCarStatus.carState = 1;
+		/*roadOri = gpsMapLine[PathPoint]->roadOri;
+		roadNow = roadOri;*/
+		busMode = false;
+
+
+       waitGpsTimeWidth= ServiceCarStatus.waitGpsTimeWidth;
+        outGarageLong=ServiceCarStatus.outGarageLong;
+		isFirstRun = false;
+	}
+    busMode=ServiceCarStatus.busmode;
+    ServiceCarStatus.useMobileEye=true;
+	gps_decition->leftlamp = false;
+	gps_decition->rightlamp = false;
+
+	
+	is_arrivaled = false;
+	
+
+
+	
+
+
+	
+
+	if (parkMode)
+	{
+
+		
+		handBrakePark(gps_decition,10000,now_gps_ins);
+		return gps_decition;
+	}
+
+
+	//驻车
+
+	if (zhucheMode)
+	{
+
+		if (trumpet()<5000)
+		{
+			gps_decition->brake=20;
+			gps_decition->accelerator = 0;
+			gps_decition->wheel_angle = 0;
+			gps_decition->speed = 0;
+			return gps_decition;
+		}
+		else
+		{
+			hasZhuched = true;
+			zhucheMode = false;
+			trumpetFirstCount = true;
+		}
+
+	}
+
+
+	//判断驻车标志位
+	if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
+	{
+		hasZhuched = false;
+	}
+
+
+
+	obsDistance = -1;
+    esrLineIndex = -1;
+	lidarDistance = -1;
+
+
+	roadPre = -1;
+
+
+
+    //realspeed = now_gps_ins.speed;
+    realspeed = ServiceCarStatus.vehSpeed_st;
+	secSpeed = realspeed / 3.6;
+
+
+    std::cout << "\n#############realspeed:\n" <<  realspeed << std::endl;
+
+	avoidX = (roadOri-roadNow)*roadWidth;
+
+	gpsTraceOri.clear();
+	gpsTraceNow.clear();
+	gpsTraceAvoid.clear();
+	gpsTraceBack.clear();
+
+
+    gpsTraceOri=getGpsTraceByMobileEye(ServiceCarStatus.Lane.curvature,ServiceCarStatus.Lane.heading,
+                                       (ServiceCarStatus.aftermarketLane.dist_to_lane_l+ServiceCarStatus.aftermarketLane.dist_to_lane_r)*0.5);
+     controlAng = iv::decition::Compute00().getDecideAngleByLanePID(realspeed);
+  //  controlAng = iv::decition::Compute00().getDecideAngle(gpsTraceOri, realspeed);
+    if(outGarageTimeLast!=-1){
+        outGarageTimeNow=GetTickCount();
+    }
+    if(outGarageTimeNow!=-1){
+        outGarageDistance+= (outGarageTimeNow-outGarageTimeLast)*secSpeed*0.001;
+         std::cout << "使用mobileEye决策————outGarageDistance:"<<outGarageDistance<< std::endl;
+    }
+    if(outGarageDistance>outGarageLong && !waitGps){
+        waitGps=true;
+
+        outGarageTimeLast=-1;
+        outGarageTimeNow=-1;
+        outGarageDistance=0;
+
+
+    }
+
+    if(ServiceCarStatus.m_bOutGarage && !waitGps){
+        outGarageTimeLast=GetTickCount();
+    }
+
+    if(waitGps){
+         std::cout << "使用mobileEye决策————waitGps"<< std::endl;
+        if(waitGpsTimeStart==-1){
+            waitGpsTimeStart=GetTickCount();
+        }
+        if(waitGpsTimeStart!=-1){
+            waitGpsTimeNow=GetTickCount();
+        }
+        if(waitGpsTimeNow!=-1){
+            waitGpsTimeSpan=waitGpsTimeNow-waitGpsTimeStart;
+            std::cout << "使用mobileEye决策————waitGps————waitGpsTimeSpan："<<waitGpsTimeSpan <<std::endl;
+        }
+        if(waitGpsTimeSpan>waitGpsTimeWidth){
+            waitGps=false;
+            waitGpsTimeStart=-1;
+            waitGpsTimeNow=-1;
+            waitGpsTimeSpan=0;
+             ServiceCarStatus.m_bOutGarage=false;
+               std::cout << "使用mobileEye决策————waitGps————退出"<< std::endl;
+        }
+
+        gps_decition->accelerator =0;
+        gps_decition->torque = 0;
+        gps_decition->brake=0.8;
+        gps_decition->wheel_angle = 0.0 - controlAng;
+        //           gps_decition->wheel_angle = 0;
+
+        return gps_decition;
+
+    }
+
+	
+
+
+
+	
+
+		
+
+  //  controlAng = iv::decition::Compute00().getDecideAngleByLane(realspeed);
+
+
+	if (readyParkMode)
+	{
+		double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
+
+	
+		if (parkDis < 25)
+		{
+			Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
+			double parkDistance = pt.y;
+
+
+			if (dSpeed > 15)
+			{
+				dSpeed = 15;
+			}
+
+
+			
+			if (parkDistance < 5 && parkDistance >= 4)
+			{
+                dSpeed = min(dSpeed, 8.0);
+			}
+
+
+
+			if (parkDistance < 1.5)
+			{
+				dSpeed = 0;
+				parkMode = true;
+				readyParkMode = false;
+			}
+
+
+		}
+	}
+
+
+
+	//准备驻车
+
+	if (readyZhucheMode)
+	{
+		double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
+
+		if (dis<25)
+		{
+			Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
+			double zhucheDistance = pt.y;
+
+
+			if (dSpeed > 15)
+			{
+				dSpeed = 15;
+			}
+
+
+
+			if (zhucheDistance < 5 && zhucheDistance >= 4)
+			{
+                dSpeed = min(dSpeed, 8.0);
+			}
+
+
+
+			if (zhucheDistance < 3)
+			{
+				dSpeed = 0;
+				zhucheMode = true;
+				readyZhucheMode = false;
+			}
+
+		}
+		
+		
+
+	}
+
+
+
+
+
+    dSpeed=15;
+
+
+	//速度结合角度的限制
+	controlAng = limitAngle(realspeed, controlAng);
+	dSpeed = limitSpeed(controlAng, dSpeed);
+
+	gps_decition->speed = dSpeed;
+
+
+	dSecSpeed = dSpeed / 3.6;
+	//	 ac = 2 * max(10, realspeed) / ((dSecSpeed*dSecSpeed) - (secSpeed*secSpeed));
+
+
+
+
+
+	/* if (obsDistance <0 && ac<0 && ac >= -0.5)
+	{
+	gps_decition->accelerator = 0;
+	gps_decition->brake = 0;
+	}
+	else {*/
+
+	
+
+
+
+
+
+
+
+
+        std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
+		
+		//停车
+        if (ServiceCarStatus.carState == 0 && busMode)  //stop
+        {
+
+            gps_decition->accelerator =0;
+            gps_decition->torque = 0;
+            gps_decition->brake=0.5;
+            gps_decition->wheel_angle = 0.0 - controlAng;
+            //           gps_decition->wheel_angle = 0;
+
+            return gps_decition;
+
+        }
+
+
+
+        if (ServiceCarStatus.carState == 3 && busMode)   //  lin stop
+        {
+
+            ServiceCarStatus.carState=0;
+            gps_decition->accelerator = 0;
+            gps_decition->torque = 0;
+            gps_decition->brake=0.5;
+            gps_decition->wheel_angle = 0.0 - controlAng;
+            return gps_decition;
+
+
+        }
+
+
+
+
+
+
+
+        if (ServiceCarStatus.carState == 2 && busMode)  // dao zhan dian
+        {
+            dSpeed=3;
+        }
+
+
+
+
+                if (now_gps_ins.ins_status !=4 ) {
+
+                    dSpeed = min(15.0, dSpeed);
+
+                }
+
+
+        dSpeed=3;
+		dSecSpeed = dSpeed / 3.6;
+		gps_decition->speed = dSpeed;
+
+
+        decitionGpstool->computeObsOnRoad(lidarGridPtr,gpsTraceOri,0,gpsMapLine,lidar_per);
+
+        int confL=ServiceCarStatus.aftermarketLane.lane_conf_left;
+         int confR=ServiceCarStatus.aftermarketLane.lane_conf_right;
+         int conf=min(confL,confR);
+         if(conf<=1){
+
+         }
+
+        phaseSpeedDecition(gps_decition, secSpeed, decitionGpstool->obsDistance, decitionGpstool->obsSpeed,now_gps_ins);
+
+    gps_decition->wheel_angle = 0.0 - controlAng;
+
+    lastAngle=gps_decition->wheel_angle;
+
+	
+	return gps_decition;
+
+
+}
+
+
+
+void iv::decition::DecideLine00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDistance, double obsSpeed, GPS_INS gpsIns) {
+
+    //      obsDistance = -1;
+
+
+    std::cout << "\n计算用obs速度:%f\n" << obsSpeed << std::endl;
+    std::cout << "\n计算用obs距离:%f\n" << obsDistance << std::endl;
+
+
+
+    //  double acc = gpsIns.accel_y;
+    //    if(dSpeed>0)
+    //    dSpeed=dSpeed;
+    if(obsDistance<5 && obsDistance>0){
+        dSpeed=0;
+    }
+
+    dSecSpeed = dSpeed / 3.6;
+
+    double vt = dSecSpeed;
+    double vs = secSpeed;
+
+
+    if (abs(vs) < 0.05) vs = 0;
+    if (abs(obsSpeed) < 0.05) obsSpeed = 0;
+
+
+
+
+    double vl = vs + obsSpeed;
+    double vh = vs;
+    double dmax = 150;
+
+    //车距滤波
+    if (obsDistance < 0||obsDistance>100) {
+        obsDistance = 500;
+        obsSpeed=0;
+    }
+    if (obsDistance < 1) obsDistance = 1;
+
+    if (obsDistance > 150) vl = 25; //25m/s
+
+    if (obsSpeed <-15) obsDistance = obsDistance * 0.25 + lastDistance * 0.75;
+    else obsDistance = obsDistance * 0.5 + lastDistance * 0.5;
+
+
+
+    //TTC计算
+    double ds = 0.2566 * vl * vl + 5;
+    double ttcr = (vh - vl) / obsDistance;
+    double ttc = 15;
+    if (15 * (vh - vl) > obsDistance)
+        ttc = obsDistance / (vh - vl);
+
+
+    ServiceCarStatus.mfttc = ttc;
+
+
+    if (obsDistance <= dmax)
+    {
+        if (ttc > 10)
+        {
+            if (obsDistance > ds + 5)
+            {
+                double dds = min(30.0, obsDistance - (ds + 5));
+                vt = vt * dds / 30 + vl * (1 - dds / 30);
+            }
+            else
+            {
+                vt = min(vt, vl);
+            }
+        }
+        else
+        {
+            vt = min(vt, vl);
+        }
+    }
+
+    //   vt=min(vt,dSecSpeed);
+    vt=dSecSpeed;
+    std::cout << "\nvt:%f\n" << vt << std::endl;
+    //速度控制
+
+
+
+
+
+    double Id = 0;
+
+
+
+
+    double ed = ds - obsDistance;
+    if (obsDistance > 150) ed = 0;
+    double ev = vs - vt;
+    double u = 0;
+
+    if (ttc>10)  //if (ttc>10)
+    {
+
+        double kp = 0.5;        //double kp = 0.5;
+        double kd = 0.3;       //kd = 0.03
+        double k11 = 0.18;      //double k11 = 0.025;
+        double k12 = 0.000125;
+
+
+        double dev = (ev - lastEv) / 0.1;
+
+        Iv = 0.925 * Iv + ev;
+
+        Id = 0.85 * Id + ed;
+
+        if (abs(vh) < 2.0&& abs(vl) < 2.0)
+        {
+            Iv = 0.0; Id = 0.0;
+        }
+
+        /*u = kp * ev + kd * dev + k11 * Iv + k12 * Id;*/
+        u = kp * ev + kd * dev;
+    }
+
+
+    else if (ttc < 1.6 || (ttc <= 10 && obsDistance <= 0.75 * ds))
+    {
+        //AEB控制
+        Id = 0; Iv = 0;
+
+        u = 0;
+        if (ttc < 0.8) u = 7;
+        else if (ttc < 1.6) u = 3 + 5 * (1.6 - ttc);
+        else
+        {
+            u = (10 - ttc) * (10 - ttc) / 23.52;
+        }
+
+    }
+    else
+    {
+        //制动控制
+        Id = 0; Iv = 0;
+
+        if (obsDistance > 1.25 * ds)
+        {
+            double rev_ed = 1 / ed;
+
+            u = -0.5 * obsDistance * obsDistance * rev_ed * ttcr * ttcr;
+        }
+        else
+        {
+            if (abs(vl) > 2.0)
+                u = 0;
+            else
+                u = max(lastU, 1.0);
+        }
+    }
+
+    if (abs(vl) < 1.0 && abs(vh) < 1.0
+            && obsDistance < 2 * ds)
+    {
+        u = 0.5;
+    }
+
+
+
+
+
+
+    //u 限值
+    if (gpsIns.ins_pitch_angle>4&&u<-2.0)
+    {
+        u = -2.0;
+    }else if(ttc<3 && u<-0.2){
+        u=-0.2;
+    }
+    else
+    {
+
+        //     if (u < -0.7) u = -0.7;
+        if (u < -1.5) u = -1.5;
+    }
+
+    //        if (ttc > 8)
+    //        {
+    //            if (u > 0.8) u = 0.8;
+    //        }
+
+    if (u >= 0 && lastU <= 0)
+    {
+        if (u > 0.5) u = 0.5;
+    }
+    else if (u >= 0 && lastU >= 0)
+    {
+        if (u > lastU + 0.5) u = lastU + 0.5;
+    }
+    else if (u <= 0 && lastU >= 0)
+    {
+        if (u < -0.04) u = -0.04;
+    }
+    else if (u <= 0 && lastU <= 0)
+    {
+        if (u < lastU - 0.04) u = lastU - 0.04;
+    }
+
+
+
+
+    lastU = u;
+    lastEv = ev;
+    lastVt = vt;
+
+    //brakeValue start
+    if (u > 0)
+    {
+
+        decition->torque=0;
+        controlBrake=u; //102
+        if(obsDistance>0 && obsDistance<10){
+            controlBrake=u*1.5;
+        }
+        if(abs(dSpeed-realspeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0){
+            controlBrake=0;
+            decition->torque=max(0.0,ServiceCarStatus.torque_st-10.0);
+        }
+        //0110
+        if(changingDangWei){
+            controlBrake=max(0.3,controlBrake);
+        }
+
+    } //brakeValue end
+    else
+    {
+        controlBrake = 0;
+
+        if(ServiceCarStatus.torque_st==0){
+            decition->torque = (u*(-16)+((0-u)-ServiceCarStatus.mfVehAcc)*10)*0.7; //*1.0
+            decition->torque =max( decition->torque,1.0f);
+
+        }else{
+            decition->torque= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfVehAcc)*10;//1115    *10
+            }
+
+        if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
+            (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
+            if(realspeed<5  ){
+                decition->torque=min((float)25.0,decition->torque); //youmenxianxiao  30
+            }else if(realspeed<10){
+                decition->torque=min((float)25.0,decition->torque);  //40
+            }
+        }
+        decition->torque = min((float)(ServiceCarStatus.torque_st+5.0),decition->torque); //1115 5.0
+
+
+        // 斜坡加大油门   0217 lsn
+
+
+
+
+        if(((gpsIns.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (gpsIns.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realspeed)<1.0){
+            decition->torque=max((float)20.0,decition->torque);
+            decition->torque=min((float)40.0,decition->torque);
+        }
+
+
+
+       else if(((gpsIns.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
+            (gpsIns.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
+                && abs(realspeed)<10.0){
+                decition->torque=min((float)40.0,decition->torque); //youmenxianxiao  30
+        }
+
+
+
+
+        // 斜坡加大油门  end
+
+        decition->torque=min((float)100.0,decition->torque);
+        decition->torque=max((float)0.0,decition->torque);
+    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+    //if (vt<2)                                                    //怠速
+    //{
+    //	controlSpeed = 0;
+    //}
+
+
+
+
+
+
+    //刹车限制
+
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else if(ttc>1.6){
+        BrakeIntMax = 5;
+    }else if(ttc>0.8){
+        BrakeIntMax = 8;
+    }else{
+        BrakeIntMax = 10;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    //    if((obsDistance<0 || obsDistance>40)&&abs(dSpeed-realspeed)<3 && controlBrake>0){
+    //        controlBrake=0;
+    //        controlSpeed = max(ServiceCarStatus.torque_st-1.0,0.0); //1115
+    //    }
+    if((obsDistance<0 || obsDistance>40)&&abs(dSpeed-realspeed)<3 && controlBrake>0 && dSpeed>10){
+        controlBrake=0;
+        controlSpeed = max(ServiceCarStatus.torque_st-1.0,1.0); //1115
+    }
+
+
+
+    decition->brake = controlBrake;
+    decition->brake=min(100.0,double(decition->brake));
+    //   decition->torque = controlSpeed;
+
+    if (obsDistance>0)
+    {
+        iv::decition::DecideGps00::lastDistance = obsDistance;
+    }
+    else
+    {
+        iv::decition::DecideGps00::lastDistance = 200;
+    }
+
+
+
+    //1115
+
+    //        if(decition->torque>0){
+    //            if(decition->torque>lastTorque){
+    //                decition->torque=min(float(lastTorque+1.0),decition->torque);
+    //            }
+    ////            else if(decition->torque<lastTorque){
+    ////                decition->torque=max(float(lastTorque-5.0), decition->torque);
+    ////            }
+    //        }
+
+    //        lastTorque=decition->torque;
+
+    //斜坡刹车加大 lsn 0217
+    if (abs(gpsIns.ins_pitch_angle)>2.5 &&(decition->brake>0||dSpeed==0)){
+        decition->brake=max(2.0f,decition->brake);
+    }
+    //斜坡刹车加大 end
+
+
+    //0227 10m nei xianzhi shache
+    if(obsDistance<10 &&obsDistance>0){
+        decition->torque=0;
+        decition->brake=max(decition->brake,0.8f);
+    }
+
+
+
+
+
+    decition->accelerator=decition->torque;
+
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+double iv::decition::DecideLine00::limitAngle(double speed, double angle) {
+	double preAngle = angle;
+
+	if (speed > 15)
+	{
+		if (preAngle > 350)
+		{
+			preAngle = 350;
+		}
+		if (preAngle < -350)
+		{
+			preAngle = -350;
+		}
+	}
+	if (speed > 22)
+	{
+		if (preAngle > 200)
+		{
+			preAngle = 200;
+		}
+		if (preAngle < -200)
+		{
+			preAngle = -200;
+		}
+	}
+	if (speed > 25)
+	{
+		if (preAngle > 150)
+		{
+			preAngle = 150;
+		}
+		if (preAngle < -150)
+		{
+			preAngle = -150;
+		}
+	}
+	if (speed > 30)
+	{
+		if (preAngle > 70)
+		{
+			preAngle = 70;
+		}
+		if (preAngle < -70)
+		{
+			preAngle = -70;
+		}
+	}
+	if (speed > 45)                     //20
+	{
+		if (preAngle > 15)
+		{
+			preAngle = 15;
+		}
+		if (preAngle < -15)
+		{
+			preAngle = -15;
+		}
+	}
+	return preAngle;
+}
+
+
+
+double iv::decition::DecideLine00::limitSpeed(double angle, double speed) {
+
+	if (abs(angle) > 500 && speed > 8) speed = 8;
+	else if (abs(angle) > 350 && speed > 14) speed = 14;
+	else if (abs(angle) > 200 && speed > 21) speed = 21;
+	else if (abs(angle) > 150 && speed > 24) speed = 24;
+	else if (abs(angle) > 60 && speed > 29) speed = 29;
+	else if (abs(angle) > 20 && speed > 34) speed = 34;
+    return max(0.0, speed);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+double iv::decition::DecideLine00::trumpet() {
+	if (trumpetFirstCount)
+	{
+		trumpetFirstCount = false;
+		trumpetLastTime= GetTickCount();
+        trumpetTimeSpan = 0.0;
+	}
+	else
+	{
+		trumpetStartTime= GetTickCount();
+		trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
+		trumpetLastTime = trumpetStartTime;
+	}
+	
+	return trumpetTimeSpan;
+}
+
+
+void  iv::decition::DecideLine00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins) {
+
+	if (abs(now_gps_ins.speed)>0.1)
+	{
+		decition->accelerator = 0;
+		decition->brake = 20;
+		decition->wheel_angle = 0;
+	
+	}
+	else
+	{
+
+		decition->accelerator = 0;
+		decition->brake = 20;
+		decition->wheel_angle = 0;
+		handPark = true;
+		handParkTime = duringTime;		
+	}
+
+}
+
+
+
+std::vector<iv::Point2D> iv::decition::DecideLine00::getGpsTraceByMobileEye(float a,float b, float c){
+    vector<iv::Point2D> trace;
+    for(int i=0; i<600;i++){
+       float y=0.1*i;
+       float x=(a*y*y+b*y+c);
+       iv::Point2D pt;
+        pt.x=x;
+        pt.y=y;
+        trace.push_back(pt);
+    }
+    return trace;
+}
+

src/decition/decition_brain_sf_add_frenet_plan/decition/decition.pri

@@ -0,0 +1,61 @@
+HEADERS += \  
+    $$PWD/decition_type.h \
+    $$PWD/decition_maker.h \
+    $$PWD/decide_gps_00.h \
+    $$PWD/brain.h \
+    $$PWD/decide_line_00.h \
+    $$PWD/obs_predict.h \
+    $$PWD/adc_tools/transfer.h \
+    $$PWD/adc_tools/gps_distance.h \
+    $$PWD/adc_tools/gnss_coordinate_convert.h \
+    $$PWD/adc_tools/compute_00.h \
+    $$PWD/adc_planner/lane_change_planner.h \
+    $$PWD/adc_planner/frenet_planner.h \
+    $$PWD/adc_planner/base_planner.h \
+    $$PWD/adc_controller/pid_controller.h \
+    $$PWD/adc_controller/base_controller.h \
+    $$PWD/adc_adapter/ge3_adapter.h \
+    $$PWD/adc_adapter/base_adapter.h \
+    $$PWD/adc_tools/parameter_status.h \
+    $$PWD/adc_adapter/qingyuan_adapter.h \
+    $$PWD/adc_adapter/vv7_adapter.h \
+    $$PWD/adc_adapter/zhongche_adapter.h \
+    $$PWD/adc_planner/dubins_planner.h \
+    $$PWD/adc_tools/dubins.h \
+    $$PWD/adc_adapter/bus_adapter.h \
+    $$PWD/fanyaapi.h \
+    $$PWD/adc_adapter/hapo_adapter.h \
+    $$PWD/adc_adapter/sightseeing_adapter.h \
+    $$PWD/adc_adapter/interpolation_2d.h \
+    $$PWD/adc_tools/PolynomialRegression.h \
+    $$PWD/adc_tools/polyfit.h \
+    $$PWD/adc_planner/spline_planner.h
+
+SOURCES += \
+    $$PWD/decide_gps_00.cpp \
+    $$PWD/brain.cpp \
+    $$PWD/decide_line_00_.cpp \
+    $$PWD/obs_predict.cpp \
+    $$PWD/adc_tools/transfer.cpp \
+    $$PWD/adc_tools/gps_distance.cpp \
+    $$PWD/adc_tools/gnss_coordinate_convert.cpp \
+    $$PWD/adc_tools/compute_00.cpp \
+    $$PWD/adc_planner/lane_change_planner.cpp \
+    $$PWD/adc_planner/frenet_planner.cpp \
+    $$PWD/adc_planner/base_planner.cpp \
+    $$PWD/adc_controller/pid_controller.cpp \
+    $$PWD/adc_controller/base_controller.cpp \
+    $$PWD/adc_adapter/ge3_adapter.cpp \
+    $$PWD/adc_adapter/base_adapter.cpp \
+    $$PWD/adc_adapter/qingyuan_adapter.cpp \
+    $$PWD/adc_adapter/vv7_adapter.cpp \
+    $$PWD/adc_adapter/zhongche_adapter.cpp \
+    $$PWD/adc_planner/dubins_planner.cpp \
+    $$PWD/adc_tools/dubins.cpp \
+    $$PWD/adc_adapter/bus_adapter.cpp \
+    $$PWD/fanyaapi.cpp \
+    $$PWD/adc_adapter/hapo_adapter.cpp \
+    $$PWD/adc_adapter/sightseeing_adapter.cpp \
+    $$PWD/adc_adapter/interpolation_2d.cc \
+    $$PWD/adc_tools/polyfit.cpp \
+    $$PWD/adc_planner/spline_planner.cpp

src/decition/decition_brain_sf_add_frenet_plan/decition/decition_maker.h

@@ -0,0 +1,62 @@
+#pragma once
+#ifndef _IV_DECITION_DECITION_MAKER_
+#define _IV_DECITION_DECITION_MAKER_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+
+namespace iv {
+	namespace decition {
+		//根据传感器传输来的信息作出决策
+		class DecitionMaker
+		{
+		public:
+			DecitionMaker();
+			~DecitionMaker();
+
+            bool isFirstRun = true;                      //第一次找点标志
+            bool isComplete = false;
+			std::vector<iv::GPS_INS> vec_Gps_Group;
+			double angleDeviation;
+			double distanceDeviation;
+			double speedDeviation;
+			double origin_x;
+			double origin_y;
+			long lastNearPointNum = 0;
+			long objPointNum = 0;
+			GPS_INS startPoint;                          //起始位置最近点
+			GPS_INS nearestGpsIns;                       //最近车前路径点
+			GPS_INS nowGpsIns;                           //当前车位置点
+			GPS_INS objGpsIns;                           //跟踪目标点
+
+			float vehLenthAdj = 0;                       //实时定位点调整长度
+
+			GPS_INS getNearestGpsIns(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data);
+
+			iv::decition::Decition getDecideForGPS(iv::GPS_INS gpsIns, const std::vector<iv::GPSData> navigation_data);
+
+			void getStartPoint(iv::GPS_INS gps_ins, const std::vector<iv::GPSData> navigation_data);
+
+			void adjustOriginPoint(iv::GPS_INS gps_ins);
+
+			double getDistanceDeviation(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
+
+			double getCrossAngle(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
+
+			double getDirectCrossAngle(iv::GPS_INS nowGPSIns, iv::GPS_INS nearestGPSIns);
+
+			float getSpeedObj(GPS_INS nowGPSIns, GPS_INS nearestGPSIns);
+
+			void checkDistance(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data,double maxDistance);
+			
+            bool checkComplete(GPS_INS nowGPSIns, const std::vector<iv::GPSData> navigation_data);
+
+		private:
+
+		};
+
+	}
+}
+
+#endif // !_IV_DECITION_DECITION_MAKER_

src/decition/decition_brain_sf_add_frenet_plan/decition/decition_type.h

@@ -0,0 +1,59 @@
+#pragma once
+#ifndef _IV_DECITION_DECITION_TYPE_
+#define _IV_DECITION_DECITION_TYPE_
+#include <common/boost.h>
+namespace iv {
+namespace decition {
+struct DecitionBasic {
+    float speed;				//车速
+    float wheel_angle;			//转向角度
+    float brake;				//刹车
+    float accelerator;			//油门
+    float torque;               //力矩
+    bool leftlamp;				//左转向灯
+    bool rightlamp;				//右转向灯
+
+    int  engine;
+    int  grade;
+    int  mode;
+    int handBrake;
+    bool speak;
+    int door;
+    bool bright;
+    int dangWei;
+
+    float angSpeed;
+    int brakeType :1;
+    char brakeEnable;  //add by fjk
+    bool left;         //add by fjk
+    bool right;        //add by fjk
+
+    bool angleEnable;
+    bool speedEnable;
+    bool dangweiEnable;
+    int  driveMode;
+
+    int directLight;
+    int brakeLight;
+    int backLight;
+    int topLight;
+
+    int farLight;
+    int nearLight;
+
+    bool   air_enable ;                  //空调使能
+     bool   air_on;
+    float   air_temp ;                  //空调温度
+    float   air_mode ;                  //空调模式
+    float   wind_level ;                  //空调风力
+    int   roof_light ;                  //顶灯
+    int   home_light ;                  //日光灯
+    float   air_worktime ;                  //空调工作时间
+    float   air_offtime ;                  //空调关闭时间
+
+};
+typedef boost::shared_ptr<DecitionBasic> Decition;	//决策
+}
+}
+#endif // !_IV_DECITION_DECITION_TYPE_
+

src/decition/decition_brain_sf_add_frenet_plan/decition/decition_voter.cpp

@@ -0,0 +1,22 @@
+#include <decition/decition_voter.h>
+
+
+iv::decition::DecitionVoter::DecitionVoter()
+{
+}
+
+iv::decition::DecitionVoter::~DecitionVoter()
+{
+}
+
+void iv::decition::DecitionVoter::decideFromAll(iv::decition::Decition & decition_final, iv::decition::Decition & decition_lidar, iv::decition::Decition & decition_radar, iv::decition::Decition & decition_camera, iv::decition::Decition & decition_gps, iv::decition::Decition & decition_localmap)
+{
+    //todo 按照优先级决定采用谁的决策  或融合采用
+    if (decition_gps != NULL) {
+        decition_final->speed = decition_gps->speed;
+        decition_final->wheel_angle = decition_gps->wheel_angle;
+        decition_final->accelerator = decition_gps->accelerator;
+        decition_final->brake = decition_gps->brake;
+    }
+
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/decition_voter.h

@@ -0,0 +1,28 @@
+#pragma once
+//对来自不同传感器所作出的决策进行加权判断  得出最终的决策（速度、角度)
+//障碍物信息-激光雷达
+//障碍物信息-毫米波雷达
+//障碍物信息-摄像头
+//局部地图规划出的路线
+//GPS 惯导和导航数据计算得出的路线
+#ifndef _IV_DECITION_VOTER_
+#define _IV_DECITION_VOTER_
+
+#include <decition/decition_type.h>
+
+namespace iv {
+	namespace decition {
+		class DecitionVoter
+		{
+		public:
+			DecitionVoter();
+			~DecitionVoter();
+
+			void decideFromAll(iv::decition::Decition & decition_final,iv::decition::Decition & decition_lidar, iv::decition::Decition & decition_radar, iv::decition::Decition & decition_camera, iv::decition::Decition & decition_gps, iv::decition::Decition & decition_localmap);
+
+		private:
+
+		};
+	}
+}
+#endif // !_IV_DECITION_VOTER_

src/decition/decition_brain_sf_add_frenet_plan/decition/fanyaapi.cpp

@@ -0,0 +1,58 @@
+#include "fanyaapi.h"
+#include <QMutex>
+
+QMutex gMutexDecison;
+qint64 gTimeDecision = 0;
+double gdecision[3];
+
+using namespace fanya;
+
+
+
+void ListenDecision(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize < 3*sizeof(double))return;
+    gMutexDecison.lock();
+    memcpy(gdecision,strdata,3*sizeof(double));
+    gTimeDecision = QDateTime::currentMSecsSinceEpoch();
+    gMutexDecison.unlock();
+}
+
+fanyaapi::fanyaapi()
+{
+    mpa = iv::modulecomm::RegisterRecv("mpcdecision",ListenDecision);
+    mpb = iv::modulecomm::RegisterSend("GPS",1000,1);
+    mpc = iv::modulecomm::RegisterSend("MAP",10000000,1);
+    mpd = iv::modulecomm::RegisterSend("desiredspeed",1000,1);
+
+}
+
+int  fanyaapi::GetDecision(double &speed, double &decison, double &wheel)
+{
+    qint64 now = QDateTime::currentMSecsSinceEpoch();
+    if((now - gTimeDecision)> 1000)
+    {
+        return -1;
+    }
+    gMutexDecison.lock();
+    speed = gdecision[0];
+    decison = gdecision[1];
+    wheel = gdecision[2];
+    gMutexDecison.unlock();
+    return 0;
+}
+
+void fanyaapi::SetGPS(GPS_INS xgps)
+{
+    iv::modulecomm::ModuleSendMsg(mpb,(char *)&xgps,sizeof(GPS_INS));
+}
+
+void fanyaapi::SetMAP(std::vector<MAP_DATA> xvectorMAP)
+{
+    iv::modulecomm::ModuleSendMsg(mpc,(char *)xvectorMAP.data(),xvectorMAP.size() *sizeof(MAP_DATA));
+}
+
+void fanyaapi::SetDesiredspeed(double fspeed)
+{
+    iv::modulecomm::ModuleSendMsg(mpd,(char *)&fspeed,sizeof(fspeed));
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/fanyaapi.h

@@ -0,0 +1,38 @@
+#ifndef FANYAAPI_H
+#define FANYAAPI_H
+
+#include <vector>
+#include "modulecomm.h"
+
+namespace fanya {
+struct GPS_INS        //惯导数据结构体
+{
+    double  gps_lat;  //纬度
+    double  gps_lng;  //经度
+    double  ins_heading; //航向
+    double  speed_y;   //纵向速度
+};
+struct MAP_DATA        //地图数据结构体
+{
+    double  gps_lat;  //纬度
+    double  gps_lng;  //经度
+    double  ins_heading; //航向
+};
+
+}
+
+
+class fanyaapi
+{
+public:
+    fanyaapi();
+    void SetGPS(fanya::GPS_INS xgps);
+    void SetMAP(std::vector<fanya::MAP_DATA> xvectorMAP);
+    void SetDesiredspeed(double fspeed);
+    int GetDecision(double & speed,double & decison, double & wheel);
+
+private:
+    void * mpa, * mpb, * mpc, * mpd;
+};
+
+#endif // FANYAAPI_H

src/decition/decition_brain_sf_add_frenet_plan/decition/gnss_coordinate_convert.cpp

@@ -0,0 +1,153 @@
+#include <decition/gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+/*
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+*/
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void GaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/gnss_coordinate_convert.h

@@ -0,0 +1,26 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/decition/decition_brain_sf_add_frenet_plan/decition/gps_distance.cpp

@@ -0,0 +1,45 @@
+#include <decition/gps_distance.h>
+#include <math.h>
+#define M_PI (3.1415926535897932384626433832795)
+
+// 计算弧度
+double iv::decition::rad(double d)
+{
+    const double PI = 3.1415926535898;
+    return d * PI / 180.0;
+}
+
+// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+double iv::decition::CalcDistance(double fLati1, double fLong1, double fLati2, double fLong2)
+{
+    const float EARTH_RADIUS = 6378.137;
+    double radLat1 = rad(fLati1);
+    double radLat2 = rad(fLati2);
+    double a = radLat1 - radLat2;
+
+    double b = rad(fLong1) - rad(fLong2);
+    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1)*cos(radLat2)*pow(sin(b / 2), 2)));
+    s = s * EARTH_RADIUS;
+    //	s = (int)(s * 10000000) / 10000;
+    return s;
+}
+
+
+// 从直角坐标两点的直线距离，单位是米
+double iv::decition::DirectDistance(double x1, double y1, double x2, double y2)
+{
+    double s = sqrt(pow((x1-x2), 2) +pow((y1-y2), 2));
+    return s;
+}
+
+
+// 从直角坐标计算两点的夹角
+double iv::decition::DirectAngle(double x1, double y1, double x2, double y2)
+{
+    double angle = atan((y2 - y1) / (x2 - x1)) * 180 / M_PI;
+    return angle;
+}
+
+
+
+

src/decition/decition_brain_sf_add_frenet_plan/decition/gps_distance.h

@@ -0,0 +1,26 @@
+#pragma once
+#ifndef _IV_DECITION_GPS_DISTANCE_
+#define _IV_DECITION_GPS_DISTANCE_
+
+namespace iv {
+	namespace decition {
+        // 计算弧度
+		double rad(double d);
+
+		// 从两个gps坐标点（经纬度）获得两点的直线距离，单位是米
+		double CalcDistance(double , double , double , double );
+
+		//计算直角坐标距离
+		double DirectDistance(double, double, double, double);
+
+		//计算直角坐标角度
+		double DirectAngle(double, double, double, double);
+
+
+	}
+}
+
+
+
+
+#endif // !_IV_DECITION_GPS_DISTANCE_

src/decition/decition_brain_sf_add_frenet_plan/decition/obs_predict.cpp

@@ -0,0 +1,118 @@
+#include <decition/obs_predict.h>
+#include <decition/decition_type.h>
+#include <common/gps_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <decition/adc_tools/transfer.h>
+#include "perception/perceptionoutput.h"
+#include<common/constants.h>
+
+using namespace std;
+#define PI (3.1415926535897932384626433832795)
+
+
+double iv::decition::PredictObsDistance( double realSpeed,std::vector<Point2D>  gpsTrace,std::vector<iv::Perception::PerceptionOutput> lidar_per){
+
+    double crashDistance=200;
+    for(int i=0;i<lidar_per.size();i++){
+        if(lidar_per[i].life>0.3 && (lidar_per[i].velocity.x!=0 || lidar_per[i].velocity.y!=0)){
+            if(lidar_per[i].location.y>20 && lidar_per[i].velocity.y>0){
+                continue;
+            }
+            else if(lidar_per[i].location.y<0 && lidar_per[i].velocity.y<0){
+                 continue;
+            }
+            else if(lidar_per[i].location.x>20 && lidar_per[i].velocity.x>0){
+                 continue;
+            }else if(lidar_per[i].location.x<-20 && lidar_per[i].velocity.x<0){
+                 continue;
+            }else{
+
+                double    crashDis = getCrashDis(realSpeed,gpsTrace,lidar_per[i]);
+
+                crashDistance=min(crashDis,crashDistance);
+            }
+
+
+        }
+
+    }
+
+
+
+
+
+    return crashDistance;
+
+
+}
+
+double iv::decition::getCrashDis(double realSpeed,std::vector<Point2D>  gpsTrace,iv::Perception::PerceptionOutput obs){
+    double dis=0;
+    int length=min(300,(int)gpsTrace.size());
+    int step=10;
+    int size =length/step;
+    for(int i=1; i<size;i++){
+        double time =  getTime( realSpeed,  gpsTrace, i*step,&dis);
+        double obsX= obs.location.x+obs.velocity.x*time;
+        double obsY= obs.location.y+obs.velocity.y*time;
+        Point2D obsP(obsX,obsY);
+        double obsDis= GetDistance(obsP,gpsTrace[i*10]);
+        if(obsDis<0.7*(ServiceCarStatus.msysparam.vehWidth+obs.size.x) ){
+            return dis;
+        }
+
+    }
+
+    return 200;
+
+}
+
+
+
+int iv::decition::getFrameCount(double realSpeed,std::vector<Point2D>  gpsTrace){
+    double lenth=0;
+    for(int i =0 ; i<gpsTrace.size()-1;i++){
+        lenth +=GetDistance(gpsTrace[i],gpsTrace[i+1]);
+    }
+
+    int frame= lenth/realSpeed;
+    frame = min(frame,50);
+
+    return frame;
+}
+
+int  iv::decition::getPoiIndex(double realSpeed,std::vector<Point2D>  gpsTrace,int frame){
+
+
+
+    double d =realSpeed*0.1*frame;
+    int index=0;
+
+    for(int j=0;j<gpsTrace.size()-1;j++){
+        double dis;
+        dis+=GetDistance(gpsTrace[j],gpsTrace[j+1]);
+        if(dis>d){
+            index=j;
+            break;
+        }
+        index=j;
+    }
+
+    return index;
+}
+
+double iv::decition::getTime(double realSpeed,std::vector<Point2D>  gpsTrace,int frame ,double *dis){
+    int size=gpsTrace.size()-1;
+    int f=min(frame,size);
+
+    for(int i=0;i<=f;i++){
+
+        *dis+=GetDistance(gpsTrace[0],gpsTrace[f]);
+
+    }
+    double time = *dis/realSpeed;
+    return time;
+
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/obs_predict.h

@@ -0,0 +1,22 @@
+#ifndef OBS_PREDICT_H
+#define OBS_PREDICT_H
+
+#include <common/gps_type.h>
+#include <common/obstacle_type.h>
+#include <decition/decition_type.h>
+#include<vector>
+#include <decition/decide_gps_00.h>
+#include "perception/perceptionoutput.h"
+
+namespace iv {
+    namespace decition {
+    //根据传感器传输来的信息作出决策
+    double PredictObsDistance(double realSpeed,std::vector<Point2D>  gpsTrace,std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    double getCrashDis(double realSpeed,std::vector<Point2D>  gpsTrace,iv::Perception::PerceptionOutput obs);
+    int getFrameCount(double realSpeed,std::vector<Point2D>  gpsTrace);
+    int getPoiIndex(double realSpeed,std::vector<Point2D>  gpsTrace , int frame);
+    double getTime(double realSpeed,std::vector<Point2D>  gpsTrace , int frame ,double *dis );
+    }
+}
+
+#endif // OBS_PREDICT_H

src/decition/decition_brain_sf_add_frenet_plan/decition/transfer.cpp

@@ -0,0 +1,138 @@
+#include <decition/transfer.h>
+#include <decition/decition_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+using namespace std;
+#define PI (3.1415926535897932384626433832795)
+
+
+
+///大地转车体
+iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
+
+    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
+    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
+
+    x_vehicle = -cos(angle) * distance;
+    y_vehicle = sin(angle) * distance;
+    return  iv::Point2D(x_vehicle, y_vehicle);
+}
+
+///车体转大地
+iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    iv::GPS_INS gps_ins;
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path, y_path);
+
+    double distance = sqrt(x_path * x_path + y_path * y_path);
+    double angle = (pos.ins_heading_angle / 180 * PI + theta);
+
+    x_vehicle = pos.gps_x + distance * sin(angle);
+    y_vehicle = pos.gps_y + distance * cos(angle);
+    gps_ins.gps_x = x_vehicle;
+    gps_ins.gps_y = y_vehicle;
+
+    return  gps_ins;
+}
+
+double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
+{
+    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
+}
+
+double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
+{
+    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
+}
+
+
+
+
+//GPS转大地坐标
+void iv::decition::BLH2XYZ(iv::GPS_INS gp)
+{
+    int nFlag = 2;
+
+    double B = gp.gps_lat;
+
+    double L = gp.gps_lng;
+
+
+
+    double N, E, h;
+    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
+
+    double a = 6378245.0;            //地球半径  北京6378245
+    double F = 298.257223563;        //地球扁率
+    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
+
+    double f = 1 / F;
+    double b = a * (1 - f);
+    double ee = (a * a - b * b) / (a * a);
+    double e2 = (a * a - b * b) / (b * b);
+    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
+    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
+    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
+    double gm = 15 * n2 / 16 - 15 * n4 / 32;
+    double dt = -35 * n3 / 48 + 105 * n5 / 256;
+    double ep = 315 * n4 / 512;
+
+    B = B * iPI;
+    L = L * iPI;
+    L0 = L0 * iPI;
+
+    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
+    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
+    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
+    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
+    double yt = e2 * cos(B) * cos(B);
+    N = lB;
+    N += t * Nn * cl2 / 2;
+    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
+    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
+    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
+
+    E = Nn * cl;
+    E += Nn * cl3 * (1 - t2 + yt) / 6;
+    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
+    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
+
+    E += 500000;
+    if (nFlag == 1)
+    {
+        //UTM投影
+        N = 0.9996 * N;
+        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
+    }
+    if (nFlag == 2)
+    {
+        //UTM投影
+        N = 0.9999 * N;
+        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
+    }
+
+    //原
+    //pt2d.x = N;
+    //pt2d.y = E;
+    //
+    gp.gps_x = E - 280000;
+    gp.gps_y = N - 4325000;
+
+}
+
+
+double iv::decition::GetL0InDegree(double dLIn)
+{
+    //3°带求带号及中央子午线经度(d的形式)
+    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
+    double L = dLIn;//d.d
+    double L_ddd_Style = L;
+    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
+    double L0 = ZoneNumber * 3.0;//degree
+    return L0;
+}

src/decition/decition_brain_sf_add_frenet_plan/decition/transfer.h

@@ -0,0 +1,27 @@
+#pragma once
+#ifndef _IV_DECITION_TRANSFER_
+#define _IV_DECITION_TRANSFER_
+
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include<vector> 
+
+namespace iv {
+    namespace decition {
+        //根据传感器传输来的信息作出决策
+
+        double GetL0InDegree(double dLIn);
+
+        void BLH2XYZ(GPS_INS gps_ins);
+
+        ///大地转车体
+        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
+
+        ///车体转大地
+        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
+        double GetDistance(Point2D x1, Point2D x2);
+        double GetDistance(GPS_INS p1, GPS_INS p2);
+    }
+}
+
+#endif // ! _IV_DECITION_TRANSFER_

src/decition/decition_brain_sf_add_frenet_plan/decition_brain_sf_add_frenet_plan.pro

@@ -0,0 +1,102 @@
+QT -= gui
+
+QT += network dbus xml
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+QMAKE_LFLAGS += -no-pie
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += $$PWD/../common/main.cpp \
+    ../../include/msgtype/brainstate.pb.cc \
+    ../../include/msgtype/decition.pb.cc \
+    ../../include/msgtype/radarobjectarray.pb.cc \
+    ../../include/msgtype/radarobject.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/mapreq.pb.cc \
+    ../../include/msgtype/hmi.pb.cc \
+    ../../include/msgtype/chassis.pb.cc \
+    ../../include/msgtype/vbox.pb.cc \
+    ../../include/msgtype/radio_send.pb.cc \
+    ../../include/msgtype/formation_map.pb.cc \
+    ../../include/msgtype/fusionobjectarray.pb.cc \
+    ../../include/msgtype/fusionobject.pb.cc \
+    ../../include/msgtype/carstate.pb.cc \
+    ../../include/msgtype/groupmsg.pb.cc
+
+
+include($$PWD/../common/common/common.pri)
+include($$PWD/decition/decition.pri)
+include($$PWD/../common/perception_sf/perception_sf.pri)
+#include(platform/platform.pri)
+
+
+INCLUDEPATH += $$PWD/../common
+
+
+
+INCLUDEPATH += $$PWD/../../include/msgtype
+
+LIBS += -lprotobuf
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+#INCLUDEPATH += $$PWD/../../../include/
+#LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog
+# -livbacktrace
+
+
+
+#win32: INCLUDEPATH += C:/File/PCL_1.8/PCL_1.8.1/3rdParty/boost-1.65/include/boost-1_65
+
+win32: INCLUDEPATH += C:/File/boost/boost_1_67_0
+win32: LIBS += -LC:/File/boost/boost_1_67_0/vc2017/lib -lboost_system-vc141-mt-x64-1_67 -lboost_thread-vc141-mt-x64-1_67
+
+
+unix:LIBS += -lboost_thread -lboost_system -lboost_serialization
+unix:INCLUDEPATH += /usr/include/eigen3
+
+unix:!macx: INCLUDEPATH += $$PWD/.
+unix:!macx: DEPENDPATH += $$PWD/.
+
+HEADERS += \
+    ../../include/msgtype/brainstate.pb.h \
+    ../../include/msgtype/decition.pb.h \
+    ../../include/msgtype/radarobjectarray.pb.h \
+    ../../include/msgtype/radarobject.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/mapreq.pb.h \
+    ../../include/msgtype/hmi.pb.h \
+    ../../include/msgtype/vbox.pb.h \
+    ../common/common/roadmode_type.h \
+    ../../include/msgtype/chassis.pb.h \
+    ../../include/msgtype/radio_send.pb.h \
+    ../../include/msgtype/formation_map.pb.h \
+    ../../include/msgtype/fusionobjectarray.pb.h \
+    ../../include/msgtype/fusionobject.pb.h \
+    ../common/perception_sf/eyes.h \
+    ../common/perception_sf/perceptionoutput.h \
+    ../common/perception_sf/sensor.h \
+    ../common/perception_sf/sensor_camera.h \
+    ../common/perception_sf/sensor_gps.h \
+    ../common/perception_sf/sensor_lidar.h \
+    ../common/perception_sf/sensor_radar.h \
+    ../common/common/sysparam_type.h \
+    ../../include/msgtype/carstate.pb.h \
+    ../../include/msgtype/groupmsg.pb.h
+
+

src/decition/decition_brain_sf_add_frenet_plan/velacctable.txt

@@ -0,0 +1,71 @@
+ 3.000	 0.536	10.000	 0.000
+ 6.000	 0.555	10.000	 0.000
+ 9.000	 0.513	10.000	 0.000
+12.000	 0.539	10.000	 0.000
+15.000	 0.514	10.000	 0.000
+18.000	 0.525	10.000	 0.000
+18.000	-0.800	 0.000	10.000
+15.000	-0.781	 0.000	10.000
+12.000	-0.750	 0.000	10.000
+ 9.000	-0.788	 0.000	10.000
+ 6.000	-0.741	 0.000	10.000
+ 3.000	-0.699	 0.000	10.000
+ 3.000	 1.069	30.000	 0.000
+ 6.000	 1.511	30.000	 0.000
+ 9.000	 1.561	30.000	 0.000
+12.000	 1.500	30.000	 0.000
+15.000	 1.554	30.000	 0.000
+18.000	 1.543	30.000	 0.000
+18.000	-1.967	 0.000	20.000
+15.000	-1.814	 0.000	20.000
+12.000	-1.786	 0.000	20.000
+ 9.000	-1.787	 0.000	20.000
+ 6.000	-1.684	 0.000	20.000
+ 3.000	-1.737	 0.000	20.000
+ 3.000	 1.088	50.000	 0.000
+ 6.000	 1.564	50.000	 0.000
+ 9.000	 1.656	50.000	 0.000
+12.000	 1.625	50.000	 0.000
+15.000	 1.605	50.000	 0.000
+18.000	 1.585	50.000	 0.000
+18.000	-3.485	 0.000	30.000
+15.000	-2.638	 0.000	30.000
+12.000	-2.831	 0.000	30.000
+ 9.000	-2.654	 0.000	30.000
+ 6.000	-2.729	 0.000	30.000
+ 3.000	-2.641	 0.000	30.000
+3.000	 0.127	 5.000	 0.000
+ 6.000	 0.124	 5.000	 0.000
+ 9.000	 0.113	 5.000	 0.000
+12.000	 0.109	 5.000	 0.000
+15.000	 0.094	 5.000	 0.000
+18.000	 0.086	 5.000	 0.000
+18.000	-0.427	 0.000	 5.000
+15.000	-0.445	 0.000	 5.000
+12.000	-0.430	 0.000	 5.000
+ 9.000	-0.416	 0.000	 5.000
+ 6.000	-0.405	 0.000	 5.000
+ 3.000	-0.400	 0.000	 5.000
+18.000	-0.321	 0.000	 3.000
+15.000	-0.307	 0.000	 3.000
+12.000	-0.289	 0.000	 3.000
+ 9.000	-0.288	 0.000	 3.000
+ 6.000	-0.266	 0.000	 3.000
+ 3.000	-0.261	 0.000	 3.000
+18.000	-0.100	 0.000	 0.000
+15.000	-0.131	 0.000	 0.000
+12.000	-0.115	 0.000	 0.000
+ 9.000	-0.105	 0.000	 0.000
+ 6.000	-0.093	 0.000	 0.000
+ 3.000	-0.081	 0.000	 0.000
+18.0	0.0	3.2	0.0
+9.0	0.0	1.6	0.0
+3.0	0.0	1.6	0.0
+0.0	0.0	0.0	0.0
+0.0	0.5	10.0	0.0
+0.0	1.5	30.0	0.0
+0.0	-0.6	0.0	10.0
+0.0	-9.0	0.0	100.0
+18.0	-9.0	0.0	100.0
+9.0	-9.0	0.0	100.0	
+

src/decition/decition_external/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/detection/detection_ndt_slam_hcp2/common/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/driver/driver_map_trace/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/driver/driver_map_xodrload/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/driver/driver_radio_collision/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/driver/driver_radio_p900/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/driver/vtd_pilot_if/vtd_pilot.cpp

@@ -26,7 +26,10 @@ vtd_pilot::vtd_pilot(std::string strfromvtd,std::string strtovtd)
 
     mpaegostate = iv::modulecomm::RegisterSend("egostate",10000,1);
 
+    mpafusion = iv::modulecomm::RegisterSend("fusion",10000000,1);
+
     mpthread = new std::thread(&vtd_pilot::threadego,this);
+    mpthreadfusion = new std::thread(&vtd_pilot::threadobject,this);
 
     memset( &sOwnObjectState, 0, sizeof( RDB_OBJECT_STATE_t ) );
 }
@@ -58,6 +61,7 @@ void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbda
 {
     int i;
     int j;
+    std::cout<<" obj size: "<<xvectorrdbdata.size()<<std::endl;
     for(i=0;i<(int)xvectorrdbdata.size();i++)
     {
         iv::vtd::rdbdata * prdbdata = &xvectorrdbdata[i];
@@ -69,54 +73,65 @@ void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbda
                 RDB_OBJECT_STATE_t xobj;
     //            std::cout<<" structure RDB_OBJECT_STATE_t size: "<<sizeof(xobj)<<"data size: "<<xrdbitem.mnpkgdatasize<<std::endl;
                 memcpy(&xobj,pitem->pkgdata().data(),sizeof(RDB_OBJECT_STATE_t));
-                std::cout<<"id:"<<xobj.base.id<<" name:"<<xobj.base.name<< " pos : x "<<xobj.base.pos.x<<" y "<<xobj.base.pos.y<<" z "<<xobj.base.pos.z<<std::endl;
-                iv::fusion::fusionobject * pobject = NULL;
-                int k;
-                for(k=0;k<xfusionarray.obj_size();k++)
+   //             std::cout<<"id:"<<xobj.base.id<<" name:"<<xobj.base.name<< " pos : x "<<xobj.base.pos.x<<" y "<<xobj.base.pos.y<<" z "<<xobj.base.pos.z<<std::endl;
+                if(xobj.base.id == 1)
                 {
-                    if(xfusionarray.mutable_obj(k)->id() == xobj.base.id)
-                    {
-                        pobject = xfusionarray.mutable_obj(k);
-                        break;
-                    }
+                    mfX = xobj.base.pos.x;
+                    mfY = xobj.base.pos.y;
+                    mfHeading = xobj.base.pos.h;
                 }
-                if(pobject  == NULL)
+                else
                 {
-                    pobject = xfusionarray.add_obj();
+                    iv::fusion::fusionobject * pobject = NULL;
+                    int k;
+                    for(k=0;k<xfusionarray.obj_size();k++)
+                    {
+                        if(xfusionarray.mutable_obj(k)->id() == xobj.base.id)
+                        {
+                            pobject = xfusionarray.mutable_obj(k);
+                            break;
+                        }
+                    }
+                    if(pobject  == NULL)
+                    {
+                        pobject = xfusionarray.add_obj();
+                    }
+                    pobject->set_id(xobj.base.id);
+                    double x,y;
+                    x = xobj.base.pos.x - mfX;
+                    y = xobj.base.pos.y - mfY;
+                    double relx,rely;
+                    double beta  = mfHeading*(-1.0);
+                    relx = x*cos(beta) - y*sin(beta);
+                    rely = x*sin(beta) + y*cos(beta);
+                    double vx,vy;
+                    vx = xobj.ext.speed.x;
+                    vy = xobj.ext.speed.y;
+                    vx = vx - mfvx;
+                    vy = vy - mfvy;
+                    double relvx,relvy;
+                    relvx = vx*cos(beta) - vy*sin(beta);
+                    relvy = vx*sin(beta) + vy*sin(beta);
+                    double fobjheading = xobj.base.pos.h;
+                    fobjheading = fobjheading - mfHeading;
+                    pobject->set_lifetime(100);
+                    pobject->set_prob(1.0);
+                    pobject->set_sensor_type(1);
+                    pobject->set_yaw(fobjheading);
+                    iv::fusion::PointXYZ * ppointxyz = new iv::fusion::PointXYZ;
+                    ppointxyz->set_x(relx);
+                    ppointxyz->set_y(rely);
+                    ppointxyz->set_z(0);
+                    iv::fusion::Dimension * pdim = new iv::fusion::Dimension;
+                    pdim->set_x(xobj.base.geo.dimX);
+                    pdim->set_y(xobj.base.geo.dimY);
+                    pdim->set_z(xobj.base.geo.dimZ);
+                    pobject->set_allocated_centroid(ppointxyz);
+                    pobject->set_allocated_dimensions(pdim);
+                    pobject->set_velocity_linear_x(sqrt(pow(relvx,2)+pow(relvy,2)));
+
+                    std::cout<<" x: "<<relx<<" y: "<<rely<<std::endl;
                 }
-                pobject->set_id(xobj.base.id);
-                double x,y;
-                x = xobj.base.pos.x - mfX;
-                y = xobj.base.pos.y - mfY;
-                double relx,rely;
-                double beta  = mfHeading*(-1.0);
-                relx = x*cos(beta) - y*sin(beta);
-                rely = x*sin(beta) + y*sin(beta);
-                double vx,vy;
-                vx = xobj.ext.speed.x;
-                vy = xobj.ext.speed.y;
-                vx = vx - mfvx;
-                vy = vy - mfvy;
-                double relvx,relvy;
-                relvx = vx*cos(beta) - vy*sin(beta);
-                relvy = vx*sin(beta) + vy*sin(beta);
-                double fobjheading = xobj.base.pos.h;
-                fobjheading = fobjheading - mfHeading;
-                pobject->set_lifetime(100);
-                pobject->set_prob(1.0);
-                pobject->set_sensor_type(1);
-                pobject->set_yaw(fobjheading);
-                iv::fusion::PointXYZ * ppointxyz = new iv::fusion::PointXYZ;
-                ppointxyz->set_x(relx);
-                ppointxyz->set_y(rely);
-                ppointxyz->set_z(0);
-                iv::fusion::Dimension * pdim = new iv::fusion::Dimension;
-                pdim->set_x(xobj.base.geo.dimX);
-                pdim->set_y(xobj.base.geo.dimY);
-                pdim->set_z(xobj.base.geo.dimZ);
-                pobject->set_allocated_centroid(ppointxyz);
-                pobject->set_allocated_dimensions(pdim);
-                pobject->set_velocity_linear_x(sqrt(pow(relvx,2)+pow(relvy,2)));
 
 
             }
@@ -129,7 +144,7 @@ void vtd_pilot::threadobject()
     std::vector<iv::vtd::rdbdata> xvectorrdbdata;
     int64_t nlastsharetime = std::chrono::system_clock::now().time_since_epoch().count();
 //    int nmaxobjid = 2;
-    int nshareinter = 100;  //100 ms share a data.
+    int nshareinter = 20;  //100 ms share a data.
     bool bshare = false;
     while(mbRun)
     {
@@ -141,6 +156,28 @@ void vtd_pilot::threadobject()
         }
         if(bshare)
         {
+            iv::fusion::fusionobjectarray  xfusionarray;
+            ConvertToObjectArray(xvectorrdbdata,xfusionarray);
+
+            if(xfusionarray.obj_size()>=1)
+            {
+
+            std::cout<<" now : "<<now/1000000<<std::endl;
+
+            int nbytesize = xfusionarray.ByteSizeLong();
+
+            std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+
+            if(xfusionarray.SerializeToArray(pstr_ptr.get(),nbytesize))
+            {
+                iv::modulecomm::ModuleSendMsg(mpafusion,pstr_ptr.get(),nbytesize);
+            }
+
+            }
+            else
+            {
+                std::cout<<"no object. "<<std::endl;
+            }
             //share object data.
             bshare = false;
             nlastsharetime = now;
@@ -163,7 +200,12 @@ void vtd_pilot::threadobject()
         {
             xvectorrdbdata.push_back(mvectorrdbdata[i]);
         }
+        mvectorrdbdata.clear();
         mmutexrdb.unlock();
+
+
+
+
     }
 }
 
@@ -180,16 +222,17 @@ void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const u
         mvectorrdbdata.push_back(xrdbdata);
         mmutexrdb.unlock();
         mcvrdb.notify_all();
-        if(xrdbdata.mrdbitem_size()>0)
-        {
-            iv::vtd::rdbitem * pitem = xrdbdata.mutable_mrdbitem(0);
-            msimFrame = pitem->simframe();
-            msimTime = pitem->simtime();
-            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
-            {
-
-            }
-        }
+ //       std::cout<<"notify"<<std::endl;
+//        if(xrdbdata.mrdbitem_size()>0)
+//        {
+//            iv::vtd::rdbitem * pitem = xrdbdata.mutable_mrdbitem(0);
+//            msimFrame = pitem->simframe();
+//            msimTime = pitem->simtime();
+//            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
+//            {
+
+//            }
+//        }
     }
 }
 

src/driver/vtd_pilot_if/vtd_pilot.h

@@ -34,10 +34,12 @@ private:
 private:
     void * mpfromvtd, * mptovtd;
     void * mpagpsimu;
+    void * mpafusion;
 
     void * mpaegostate;
 
     std::thread * mpthread;
+    std::thread * mpthreadfusion;
 
 <<<<<<< HEAD
     double mflat0 = 39.1207274;

src/driver/vtd_rdb/rdbconn.cpp

@@ -108,9 +108,10 @@ void RDBConn::threadconn(char *strserip, int nport)
 
     while(mbthreadconn)
     {
-        std::cout<<"start connect server."<<std::endl;
+
         if(bConnect == false)
         {
+            std::cout<<"start connect server."<<std::endl;
             if (connect( sClient, (struct sockaddr *)&server, sizeof( server ) ) == -1 )
             {
                 std::cout<<"VTD connect() failed. Server:"<<strserip<<" port:"<<nport<< std::endl;

src/tool/data_serials/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/tool/ivmapmake/common/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/tool/ivmapmake_sharemem/common/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/tool/map_lanetoxodr/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/tool/map_lanetoxodr_graphscene/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/tool/tool_ivddecode/main.cpp

@@ -0,0 +1,11 @@
+#include "mainwindow.h"
+#include <QApplication>
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    MainWindow w;
+    w.show();
+
+    return a.exec();
+}

src/tool/tool_ivddecode/mainwindow.cpp

@@ -0,0 +1,293 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+#include <QFileDialog>
+#include <QMessageBox>
+
+#include <iostream>
+
+#include "google/protobuf/io/zero_copy_stream_impl.h"
+#include "google/protobuf/text_format.h"
+
+MainWindow::MainWindow(QWidget *parent) :
+    QMainWindow(parent),
+    ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+
+    mpTimer = new QTimer();
+    connect(mpTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
+    mpTimer->setInterval(10);
+
+    connect(this,SIGNAL(CurState(int)),this,SLOT(onCurState(int)));
+
+    ui->progressBar->setRange(0,100);
+    ui->progressBar->setValue(0);
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}
+
+void MainWindow::on_pushButton_decode_clicked()
+{
+    QString strdir = QFileDialog::getExistingDirectory(this, tr("Set Save Directory"), ".", QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
+    if(strdir.isEmpty())
+    {
+        return;
+
+    }
+
+    std::cout<<" directory: "<<strdir.toStdString()<<std::endl;
+
+    QString strivd = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)"));
+    if(strivd.isEmpty())return;
+
+    mpthread = new std::thread(&MainWindow::threadconvert,this, strivd,strdir);
+    mbRunning = true;
+    ui->pushButton_decode->setEnabled(false);
+
+    mpTimer->start();
+
+
+}
+
+void MainWindow::onCurState(int nstate)
+{
+    if(nstate == -1)
+    {
+        QMessageBox::warning(this,tr("Warning"),tr("Open File Fail."),QMessageBox::YesAll);
+
+    }
+
+    if(nstate == 0)
+    {
+        ui->progressBar->setValue(100);
+    }
+
+    ui->pushButton_decode->setEnabled(true);
+    mpTimer->stop();
+}
+
+void MainWindow::onTimer()
+{
+    qint64 nSize = mnFileSize;
+    if(nSize<=0)return;
+
+    int nPos = mnPos*100/mnFileSize;
+    ui->progressBar->setValue(nPos);
+}
+
+void MainWindow::threadconvert(QString strfilepath,QString strdir)
+{
+    mFile.setFileName(strfilepath);
+    if(mFile.open(QFile::ReadOnly))
+    {
+        mbOpen = true;
+        mnFileSize = mFile.size();
+        mnPos = 0;
+    }
+    else
+    {
+        mnFileSize = 0;
+        mbReplay = false;
+        mnPos = 0;
+        mbOpen = false;
+    }
+
+    if(mbOpen == false)
+    {
+        emit onCurState(-1);
+        return;
+    }
+
+    bool bx = true;
+    while(bx == true)
+    {
+        int nReadSize = 0;
+        int nDataSize;
+        char * strData;
+        char * strName;
+        bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
+
+        mnPos = mnPos + nReadSize;
+        if(bx == true)
+        {
+            std::cout<<" read a record."<<std::endl;
+
+
+            std::cout<<" name : "<<strName<<std::endl;
+            if(strncmp(strName,"lidar_pc",256)==0)
+            {
+                QString strpath = strdir + "/" + mdtcurpos.toString("yyyy-MM-dd-hh-mm-ss-zzz") + ".pcd";
+                savepointcloud(strData,nDataSize,strpath);
+            }
+
+            if(strncmp(strName,"hcp2_gpsimu",256)==0)
+            {
+                QString strpath = strdir + "/" + mdtcurpos.toString("yyyy-MM-dd-hh-mm-ss-zzz") + ".txt";
+                savegps(strData,nDataSize,strpath);
+            }
+
+
+            delete strData;
+            delete strName;
+        }
+
+    }
+    std::cout<<"complete."<<std::endl;
+
+    emit onCurState(0);
+
+}
+
+inline QDateTime MainWindow::GetDateTimeFromRH(iv::RecordHead rh)
+{
+    QDateTime dt;
+    QDate datex;
+    QTime timex;
+    datex.setDate(rh.mYear,rh.mMon,rh.mDay);
+    timex.setHMS(rh.mHour,rh.mMin,rh.mSec,rh.mMSec);
+    dt.setDate(datex);
+    dt.setTime(timex);
+
+    return dt;
+}
+
+inline bool MainWindow::ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData)
+{
+    char strmark[10];
+    int nTotalSize,nHeadSize,nNameSize,nDataSize;
+    int nRead = mFile.read(strmark,1);
+    if(nRead == 0)return false;
+    nRead = mFile.read((char *)&nTotalSize,sizeof(int));
+    if(nRead < (int)sizeof(int))return false;
+    nRead = mFile.read((char *)&nHeadSize,sizeof(int));
+    if(nRead < (int)sizeof(int))return false;
+    nRead = mFile.read((char *)&nNameSize,sizeof(int));
+    if(nRead< (int)sizeof(int))return false;
+    nRead = mFile.read((char *)&nDataSize,sizeof(int));
+    if(nRead< (int)sizeof(int))return false;
+    if(nTotalSize !=(nHeadSize + nNameSize + nDataSize + 4*(int)sizeof(int) ))
+    {
+        return false;
+    }
+    iv::RecordHead rh;
+    char * strName = new char[1000];
+    char * strData = new char[nDataSize];
+
+    nRead = mFile.read((char *)&rh,sizeof(iv::RecordHead));
+    if(nRead < (int)sizeof(iv::RecordHead))
+    {
+        delete strData;
+        delete strName;
+        return false;
+    }
+
+    mdtcurpos = GetDateTimeFromRH(rh);
+
+    nRead = mFile.read(strName,nNameSize);
+    if(nRead < nNameSize)
+    {
+        delete strData;
+        delete strName;
+        return false;
+    }
+    strName[nNameSize] = 0;
+ //   qDebug(strName);
+//    qDebug("file pos is %d ms is %d",mFile.pos(),rh.mMSec);
+
+    nRead = mFile.read(strData,nDataSize);
+    if(nRead < nDataSize)
+    {
+        delete strData;
+        delete strName;
+        return false;
+    }
+
+    //Share Data
+
+    *pnDataSize = nDataSize;
+    *pstrName = strName;
+    *pstrData = strData;
+
+
+//    delete strData;
+    nRecSize = nTotalSize + 1;
+    return true;
+
+}
+
+void MainWindow::savepointcloud(const char *strdata, const unsigned int nSize, QString strfilepath)
+{
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        xp.x = p->x;
+        xp.y = p->y;
+        xp.z = p->z;
+        xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+
+  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
+    }
+
+    if(0 == pcl::io::savePCDFile(strfilepath.toLatin1().data(),*point_cloud))
+    {
+
+    }
+    else
+    {
+        std::cout<<" save pcd file fail. "<<std::endl;
+    }
+}
+
+void MainWindow::savegps(const char *strdata, const unsigned int nSize, QString strfilepath)
+{
+    iv::gps::gpsimu xgpsimu;
+    if(xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        using google::protobuf::TextFormat;
+        using google::protobuf::io::FileOutputStream;
+        using google::protobuf::io::ZeroCopyOutputStream;
+        std::string strout;
+        ZeroCopyOutputStream *output = new google::protobuf::io::StringOutputStream(&strout);//new FileOutputStream(file_descriptor);
+
+        bool success = TextFormat::Print(xgpsimu, output);
+        if(success)
+        {
+            QFile xFile;
+            xFile.setFileName(strfilepath);
+            if(xFile.open(QIODevice::ReadWrite))
+            {
+                xFile.write(strout.data(),strout.size());
+                xFile.close();
+            }
+ //           std::cout<<strout<<std::endl;
+//               qDebug(strout.data());
+        }
+    }
+}
+

src/tool/tool_ivddecode/mainwindow.h

@@ -0,0 +1,93 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include <QFile>
+#include <QMutex>
+#include <QDateTime>
+#include <QTimer>
+
+#include <thread>
+
+#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include "gpsimu.pb.h"
+
+namespace iv {
+    struct RecordHead
+    {
+        int mnDataIndex;
+        unsigned short mYear;
+        unsigned char mMon;
+        unsigned char mDay;
+        unsigned char mHour;
+        unsigned char mMin;
+        unsigned char mSec;
+        unsigned short mMSec;
+        int mnTime;
+    };
+}
+
+namespace Ui {
+class MainWindow;
+}
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    explicit MainWindow(QWidget *parent = 0);
+    ~MainWindow();
+
+signals:
+    void CurState(int);
+
+private slots:
+    void on_pushButton_decode_clicked();
+    void onCurState(int );
+    void onTimer();
+
+private:
+    Ui::MainWindow *ui;
+
+    std::thread * mpthread;
+    bool mbRunning;
+
+private:
+    QFile mFile;
+    bool mbOpen;
+    qint64 mnPos;
+    qint64 mnFileSize = 0;
+    bool mbReplay;
+    QMutex mMutex;
+
+    QDateTime mdtstart;
+    QDateTime mdtcurpos;
+    QTimer mTimerUpdate;
+    QTime mTime;
+
+    QDateTime mdtrpstart;
+
+    double mrpspeed = 1.0;
+
+    QTimer * mpTimer;
+
+private:
+    void threadconvert(QString strfilepath,QString strdir);
+
+    inline QDateTime GetDateTimeFromRH(iv::RecordHead rh);
+
+    inline bool ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData);
+
+    void savepointcloud(const char * strdata,const unsigned int nSize,QString strfilepath);
+
+    void savegps(const char * strdata,const unsigned int nSize,QString strfilepath);
+};
+
+#endif // MAINWINDOW_H

src/tool/tool_ivddecode/mainwindow.ui

@@ -0,0 +1,67 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>547</width>
+    <height>405</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralWidget">
+   <widget class="QPushButton" name="pushButton_decode">
+    <property name="geometry">
+     <rect>
+      <x>80</x>
+      <y>40</y>
+      <width>141</width>
+      <height>61</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>Decode</string>
+    </property>
+   </widget>
+   <widget class="QProgressBar" name="progressBar">
+    <property name="geometry">
+     <rect>
+      <x>80</x>
+      <y>130</y>
+      <width>391</width>
+      <height>51</height>
+     </rect>
+    </property>
+    <property name="value">
+     <number>24</number>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menuBar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>547</width>
+     <height>28</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QToolBar" name="mainToolBar">
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+  </widget>
+  <widget class="QStatusBar" name="statusBar"/>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources/>
+ <connections/>
+</ui>

src/tool/tool_ivddecode/tool_ivddecode.pro

@@ -0,0 +1,71 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2022-07-05T16:27:38
+#
+#-------------------------------------------------
+
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+TARGET = tool_ivddecode
+TEMPLATE = app
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+
+SOURCES += \
+        main.cpp \
+        mainwindow.cpp \
+    ../../include/msgtype/gpsimu.pb.cc
+
+HEADERS += \
+        mainwindow.h \
+    ../../include/msgtype/gpsimu.pb.h
+
+FORMS += \
+        mainwindow.ui
+
+
+
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../../src/include/msgtype
+
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+

src/tool/tool_mapcreate/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/tool/tool_xodrobj/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/tool/tracetoxodr/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -80,24 +87,30 @@ namespace iv {
     };
 
     typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
+    class Point2D
+   {
+     public:
+        double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+        int v1 = 0, v2 = 0;
+        int roadMode = 0;
+        int obs_type=0;
+
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
 
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
 
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
+        Point2D()
+       {
+           x = y = v1 = 0;
+       }
 
+        Point2D(double _x, double _y)
+       {
+           x = _x; y = _y;
+       }
 
-     };
+
+    };
 
 
 

src/ui/ui_ads_hmi/common/gps_type.h

@@ -19,6 +19,13 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -88,6 +95,9 @@ namespace iv {
         int roadMode = 0;
         int obs_type=0;
 
+        double s=0;   //frenet coordinate : s,d;   add at 20220209
+        double d=0;
+
 
         Point2D()
        {