modify avoid pro

src/decition/common/common/car_status.h

@@ -155,7 +155,7 @@ namespace iv {
         double mfEPSOff = 0.0;
         float socfDis=15;   //停障触发距离
         float aocfDis=25;   //避障触发距离
-        float aocfTimes=5; //避障触发次数
+        float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90
         int iTrafficeLightID = 0;       //红绿灯ID

src/decition/common/perception/impl_gps.cpp

@@ -1,5 +1,5 @@
 #include <perception/sensor_gps.h>
-#include <decision/gnss_coordinate_convert.h>
+#include <decition/gnss_coordinate_convert.h>
 #include <control/can.h>
 #include <QDebug>
 #include <QNetworkDatagram>

src/decition/common/perception_sf/impl_lidar.cpp

@@ -39,16 +39,6 @@ void iv::perception::LiDARSensor::updataFusion(std::shared_ptr<iv::fusion::fusio
     fusion_obs.swap(mfusion_obs_);
     mMutex.unlock();
 
-//    ++count ;
-//    if(count/epoch ==0 )
-//    {
-//        count = 0;
-
-
-//    }
-
-
-
 
     fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
     memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));

src/decition/decition_brain/decision/adc_adapter/base_adapter.cpp

@@ -1,26 +0,0 @@
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-
-
-
-
-iv::decition::BaseAdapter::BaseAdapter(){
-
-}
-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/decision/adc_adapter/base_adapter.h

@@ -1,42 +0,0 @@
-#ifndef BASE_ADAPTER_H
-#define BASE_ADAPTER_H
-
-
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.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:
-
-
-        };
-
-    }
-}
-
-
-
-
-
-#endif // BASE_ADAPTER_H

src/decition/decition_brain/decision/adc_adapter/bus_adapter.cpp

@@ -1,325 +0,0 @@
-#include <decision/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/decision/adc_adapter/bus_adapter.h

@@ -1,43 +0,0 @@
-#ifndef BUS_ADAPTER_H
-#define BUS_ADAPTER_H
-
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_adapter/ge3_adapter.cpp

@@ -1,193 +0,0 @@
-#include <decision/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);
-
-    (*decition)->air_enable=true;
-
-
-
-    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/decision/adc_adapter/ge3_adapter.h

@@ -1,60 +0,0 @@
-#ifndef GE3_ADAPTER_H
-#define GE3_ADAPTER_H
-
-
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_adapter/hapo_adapter.cpp

@@ -1,362 +0,0 @@
-#include <decision/adc_adapter/hapo_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-#include <QTime>
-
-QTime doorOpenTime;
-
-using namespace std;
-
-iv::decition::HapoAdapter::HapoAdapter(){
-    adapter_id=5;
-    adapter_name="hapo";
-}
-iv::decition::HapoAdapter::~HapoAdapter(){
-
-}
-
-#include "ivlog.h"
-extern iv::Ivlog * givlog;
-
-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<6){
-            controlBrake= u*1.5;
-        }
-         if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && dSpeed>0 && turn_around
-                 && dSpeed>0 && lastBrake==0){
-            controlBrake=0;
-            controlSpeed=0;
-        }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>5 && 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>5
-                  && !turn_around ){
-            controlBrake=min(controlBrake,0.3f);
-            controlSpeed=0;
-        }
-        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>5
-                && dSpeed>0 && !turn_around ){
-            controlBrake=min(controlBrake,0.5f);
-            controlSpeed=0;
-        }
-         else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>=15
-                 && dSpeed>0 && !turn_around ){
-             controlBrake=0;
-             controlSpeed= 2.0* (realSpeed/15.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);
-             controlSpeed =max( controlSpeed,1.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+2.0f,controlSpeed);
-        }else if(controlSpeed<ServiceCarStatus.torque_st){
-            controlSpeed = ServiceCarStatus.torque_st-1.0f;
-        }
-
-
-
-
-        //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
-//        }
-
-    }
-//0125
-    if(controlSpeed>0){
-        controlSpeed=max(controlSpeed,2.4f);
-    }
-
-    //0227 10m nei xianzhi shache
-    if(obsDistance<6 &&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*6;
-
-    (*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)->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);
-
-
-
-
-        //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;
-
-     if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
-     {
-          (*decition)->mode=0;
-     }
-
-
-     if(ServiceCarStatus.station_control_door==0)
-     {
-         (*decition)->door=false;      //CLOSE
-         doorOpenTime.start();
-         givlog->debug("DOOR","STATUS is: %d",5);
-
-     }else if((ServiceCarStatus.station_control_door==1)&&(ServiceCarStatus.station_control_door_option==false))
-     {
-         (*decition)->door=true;       //OPEN
-         (*decition)->brake =6;
-         (*decition)->torque=0;
-         if(doorOpenTime.elapsed()>6000)
-         {
-             ServiceCarStatus.station_control_door_option=true;
-             givlog->debug("DOOR","STATUS is: %d",8);
-         }
-     }
-
-givlog->debug("DOOR","door is: %d",(*decition)->door);
-givlog->debug("DOOR","station_control_door is: %d",ServiceCarStatus.station_control_door);
-
-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 ;
-
-
-givlog->debug("obs_distance","dSpeed: %f, realSpeed: %f, brake: %f",
-              dSpeed, realSpeed,(*decition)->brake);
-
-    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/decision/adc_adapter/hapo_adapter.h

@@ -1,43 +0,0 @@
-#ifndef HAPO_ADAPTER_H
-#define HAPO_ADAPTER_H
-
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_adapter/qingyuan_adapter.cpp

@@ -1,312 +0,0 @@
-#include <decision/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/decision/adc_adapter/qingyuan_adapter.h

@@ -1,41 +0,0 @@
-#ifndef QINGYUAN_ADAPTER_H
-#define QINGYUAN_ADAPTER_H
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_adapter/vv7_adapter.cpp

@@ -1,150 +0,0 @@
-#include <decision/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/decision/adc_adapter/vv7_adapter.h

@@ -1,44 +0,0 @@
-#ifndef VV7_ADAPTER_H
-#define VV7_ADAPTER_H
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_adapter/yuhesen_adapter.cpp

@@ -1,153 +0,0 @@
-#include <decision/adc_adapter/yuhesen_adapter.h>
-#include <common/constants.h>
-#include <common/car_status.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-
-using namespace std;
-
-
-iv::decition::YuHeSenAdapter::YuHeSenAdapter(){
-    adapter_id= 7;
-    adapter_name="yuhesen";
-}
-iv::decition::YuHeSenAdapter::~YuHeSenAdapter(){
-
-}
-
-
-
-iv::decition::Decition iv::decition::YuHeSenAdapter::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)-40.0,(*decition)->wheel_angle);
-     (*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);
-
-
-        (*decition)->brake=0;
-
-
-
-
-std::cout<<"==========================================="<<std::endl;
-     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim
-    <<std::endl;
-std::cout<<"========================================="<<std::endl;
-(*decition)->speed = dSpeed/3.6;
-(*decition)->wheel_angle=(*decition)->wheel_angle*0.1;
-(*decition)->wheel_angle=max((float)-40.0,(*decition)->wheel_angle);
-(*decition)->wheel_angle=min((float)40.0,(*decition)->wheel_angle);
-(*decition)->mode=1;
-(*decition)->dangWei=1;
-if((*decition)->brake>0){
-    (*decition)->speed=0;
-}
-    return *decition;
-}
-
-
-
-float iv::decition::YuHeSenAdapter::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::YuHeSenAdapter::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/decision/adc_adapter/yuhesen_adapter.h

@@ -1,44 +0,0 @@
-#ifndef YUHESEN_ADAPTER_H
-#define YUHESEN_ADAPTER_H
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.h>
-
-
-namespace iv {
-     namespace decition {
-         class YuHeSenAdapter: public BaseAdapter {
-                    public:
-
-                        float lastTorque;
-                        float lastBrake;
-                        int lastDangWei=0;
-                        int seizingCount=0;
-
-                        YuHeSenAdapter();
-                        ~YuHeSenAdapter();
-
-                        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/decision/adc_adapter/zhongche_adapter.cpp

@@ -1,179 +0,0 @@
-
-#include <decision/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)){
-
-
-
-
-
-        if(DecideGps00::minDecelerate<0){
-            (*decition)->torque = 0;
-            (*decition)->brake = max((0-DecideGps00::minDecelerate)*90, (*decition)->brake);
-             dSpeed=10.0;
-        }else{
-            (*decition)->torque=90;
-            (*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/decision/adc_adapter/zhongche_adapter.h

@@ -1,42 +0,0 @@
-#ifndef ZHONGCHE_ADAPTER_H
-#define ZHONGCHE_ADAPTER_H
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_controller/base_controller.cpp

@@ -1,39 +0,0 @@
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/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/decision/adc_controller/base_controller.h

@@ -1,67 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION__BASE_CONTROLLER_
-#define _IV_DECITION__BASE_CONTROLLER_
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include<vector>
-#include <decision/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/decision/adc_controller/pid_controller.cpp

@@ -1,352 +0,0 @@
-#include <decision/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;
-
-    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 if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
-        if(turning){
-           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);
-        }
-    } 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;
-        }
-    }
-
-    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[gpsIndex]);
-    }
-    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(ServiceCarStatus.msysparam.mvehtype=="hapo")
-    {
-            static int obs_line=0;
-            if(obsDistance<6 && obsDistance>0){
-                dSpeed=0;
-                obs_line=1;
-            }
-            if(obs_line==1)
-            {
-                if(obsDistance<8 && obsDistance>0){
-                    dSpeed=0;
-                }else{
-                    obs_line=0;
-                }
-            }
-    }else if(obsDistance<10 && 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.5
-        //      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/decision/adc_controller/pid_controller.h

@@ -1,76 +0,0 @@
-#ifndef _IV_DECITION__PID_CONTROLLER_
-#define _IV_DECITION__PID_CONTROLLER_
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include<vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_controller/base_controller.h>
-#include <decision/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/decision/adc_planner/base_planner.cpp

@@ -1,40 +0,0 @@
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_tools/transfer.h>
-#include "perception/perceptionoutput.h"
-#include <decision/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/decision/adc_planner/base_planner.h

@@ -1,48 +0,0 @@
-#ifndef _IV_DECITION__BASE_PLANNER_
-#define _IV_DECITION__BASE_PLANNER_
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include<vector>
-#include <decision/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/decision/adc_planner/dubins_planner.cpp

@@ -1,57 +0,0 @@
-#include <decision/adc_planner/dubins_planner.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_planner/dubins_planner.h

@@ -1,44 +0,0 @@
-#ifndef DUBINS_PLANNER_H
-#define DUBINS_PLANNER_H
-#include "base_planner.h"
-#include <decision/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/decision/adc_planner/frenet_planner.cpp

@@ -1,643 +0,0 @@
-#include "frenet_planner.h"
-
-#include <decision/adc_tools/transfer.h>
-#include <common/car_status.h>
-#include <decision/adc_tools/parameter_status.h>
-#include <decision/decide_gps_00.h>
-#include <decision/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/decision/adc_planner/frenet_planner.h

@@ -1,159 +0,0 @@
-#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/decision/adc_planner/lane_change_planner.cpp

@@ -1,196 +0,0 @@
-#include <decision/adc_planner/lane_change_planner.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/adc_tools/parameter_status.h>
-#include <common/constants.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/decision/adc_planner/lane_change_planner.h

@@ -1,38 +0,0 @@
-#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/decision/adc_tools/compute_00.cpp

@@ -1,2075 +0,0 @@
-#include <decision/adc_tools/compute_00.h>
-#include <decision/decide_gps_00.h>
-#include <decision/adc_tools/gps_distance.h>
-#include <decision/decition_type.h>
-#include <decision/adc_tools/transfer.h>
-#include <common/constants.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-#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::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)<=3.5)
-                                     {
-                                           (*gpsMap[MarkingIndex]).roadMode=14;   //弯道，2米,14
-                                     }
-                                     else if((BackAveFive-FrontAveFive)>3.5)
-                                     {
-                                           (*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::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)(lastIndex+map_size));
-
-
-    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(maxAngle, 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;
-                    }
-                }
-            }
-
-            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;
-
-                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;
-    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)
-                {
-                    return i;
-
-
-                    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/decision/adc_tools/compute_00.h

@@ -1,101 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_COMPUTE_00_
-#define _IV_DECITION_COMPUTE_00_
-
-#include <common/gps_type.h>
-#include <common/obstacle_type.h>
-#include <decision/decition_type.h>
-#include <vector>
-#include <decision/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 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 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/decision/adc_tools/dubins.cpp

@@ -1,439 +0,0 @@
-/*
- * 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/decision/adc_tools/dubins.h

@@ -1,149 +0,0 @@
-#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/decision/adc_tools/gnss_coordinate_convert.cpp

@@ -1,154 +0,0 @@
-#include <decision/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/decision/adc_tools/gnss_coordinate_convert.h

@@ -1,26 +0,0 @@
-#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/decision/adc_tools/gps_distance.cpp

@@ -1,45 +0,0 @@
-#include <decision/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/decision/adc_tools/gps_distance.h

@@ -1,26 +0,0 @@
-#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/decision/adc_tools/parameter_status.h

@@ -1,109 +0,0 @@
-#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/decision/adc_tools/transfer.cpp

@@ -1,138 +0,0 @@
-#include <decision/adc_tools/transfer.h>
-#include <decision/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/decision/adc_tools/transfer.h

@@ -1,27 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_TRANSFER_
-#define _IV_DECITION_TRANSFER_
-
-#include <common/gps_type.h>
-#include <decision/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/decision/brain.cpp

@@ -1,1404 +0,0 @@
-#include <decision/brain.h>
-#include <fstream>
-#include <time.h>
-#include <exception>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <common/logout.h>
-#include <decision/adc_tools/transfer.h>
-
-#include <iostream>
-#include "xmlparam.h"
-#include "qcoreapplication.h"
-#include <QTime>
-
-QTime  gps_update_times,gps_last_to_current;
-
-extern std::string gstrmemdecition;
-extern std::string gstrmembrainstate;
-extern iv::Ivlog * givlog;
-extern std::string gstrmemchassis;
-
-#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)
-        {
-            (void)&index;
-            (void)dt;
-            (void)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)
-        {
-            (void)&index;
-            (void)dt;
-            (void)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)
-        {
-            (void)&index;
-            (void)dt;
-            (void)strmemname;
-            gbrain->UpdateHMI(strdata,nSize);
-        }
-
-        void ListenPAD(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            (void)&index;
-            (void)dt;
-            (void)strmemname;
-            gbrain->UpdatePAD(strdata,nSize);
-        }
-
-        void ListenPlatform(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            (void)&index;
-            (void)dt;
-            (void)strmemname;
-            gbrain->UpdatePlatform(strdata,nSize);
-        }
-
-        void ListenGroup(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            (void)&index;
-            (void)dt;
-            (void)strmemname;
-            gbrain->UpdateGroupInfo(strdata,nSize);
-        }
-
-        void Listenv2x(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            (void)&index;
-            (void)dt;
-            (void)strmemname;
-            gbrain->Updatev2x(strdata,nSize);
-        }
-
-        void Listenultra(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-        {
-            (void)&index;
-            (void)dt;
-            (void)strmemname;
-            gbrain->Updateultra(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()
-{
-    //    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;
-    //    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);
-
-
-    mpaToPlatform = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
-    mpaDecition = iv::modulecomm::RegisterSend(gstrmemdecition.data(),10*sizeof(iv::decition::DecitionBasic),1);
-
-    mpaPAD = iv::modulecomm::RegisterRecv("pad",iv::decition::ListenPAD);
-    mpaHMI = iv::modulecomm::RegisterRecv("hmi",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);
-
-    v2x = iv::modulecomm::RegisterRecv("v2x",iv::decition::Listenv2x);
-    ultra = iv::modulecomm::RegisterRecv("ultra",iv::decition::Listenultra);
-
-    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);
-    void * pa = iv::modulecomm::RegisterRecvPlus(gstrmemchassis.data(),funchassis);
-
-    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()
-{
-}
-
-
-void iv::decition::BrainDecition::inialize() {
-    //	controller->inialize();
-    eyes->inialize();
-    //    carcall->start();
-}
-
-
-void iv::decition::BrainDecition::run() {
-
-    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","yuhesen");
-
-    ServiceCarStatus.msysparam.vehWidth = atof(xp.GetParam("vehWidth","2.1").data());
-    ServiceCarStatus.msysparam.vehLenth = atof(xp.GetParam("vehLenth","4.6").data());
-
-
-    /*=============     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("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 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
-
-
-    adjuseGpsLidarPosition();
-
-    if(strexternmpc == "true")
-    {
-        mbUseExternMPC = 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;
-    }
-
-    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;
-    }
-
-    //map
-
-    mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
-
-
-    while (eyes->isAllSensorRunning())
-    {
-        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(obs_lidar_grid_ != NULL)
-        {
-            if(old_obs_lidar_grid_ != NULL) free(old_obs_lidar_grid_);
-            old_obs_lidar_grid_ = obs_lidar_grid_;
-        }
-
-        obs_lidar_grid_ = NULL;
-
-        if(obs_camera_ != NULL)free(obs_camera_);
-
-        obs_camera_ = NULL;
-
-        int gps_update_period=0,gps_last_to_current_times=0;
-
-        gps_update_times.start();
-        eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
-        if(gps_data_!=NULL)
-        {
-           gps_update_period=gps_update_times.elapsed();
-           gps_last_to_current.start();
-        }
-        gps_last_to_current_times=gps_last_to_current.elapsed();
-
-
-
-
-
-
-        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;
-        //        }
-
-        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 (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;
-
-        givlog->debug("GPS","gps_data_ is: %d,gps_update_period is: %d,gps_last_to_current_times is: %d",gps_data_,gps_update_period,gps_last_to_current_times);
-
-        if (gps_data_  &&!handPark && !ServiceCarStatus.m_bOutGarage){// && obs_lidar_grid_) {	//处理GPS数据
-
-            double start = GetTickCount();
-
-            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_;
-            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);
-
-            mMutexMap.lock();
-            decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================
-            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;
-                }
-            }
-            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);
-
-            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;
-            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::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);
-            *data = x;
-            navigation_data.push_back(data);
-
-            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);}
-        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);
-
-
-
-    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::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));
-
-}
-
-void iv::decition::BrainDecition::UpdatePAD(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));
-
-}
-
-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::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<<"******* xchassis.torque:"<< xchassis.torque()<<std::endl;
-        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::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::Updatev2x(const char *pdata, const int ndatasize){
-
-    iv::v2x::v2x v2x_message;
-    if(false == v2x_message.ParseFromArray(pdata,ndatasize))
-    {
-        std::cout<<"Listenv2x fail."<<std::endl;
-        return;
-    }
-
-//  解析v2x信息
-    ServiceCarStatus.stationCmd.received=true;
-
-    ServiceCarStatus.stationCmd.has_carID=v2x_message.has_carid();
-    if(ServiceCarStatus.stationCmd.has_carID)
-    {
-            ServiceCarStatus.stationCmd.carID=v2x_message.carid();
-    }
-
-    ServiceCarStatus.stationCmd.has_carMode=v2x_message.has_carmode();
-    if(ServiceCarStatus.stationCmd.has_carMode)
-    {
-            ServiceCarStatus.stationCmd.carMode=v2x_message.carmode();
-
-            qDebug("ServiceCarStatus.stationCmd.carMode:",ServiceCarStatus.stationCmd.carMode);
-            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.carMode:%d",ServiceCarStatus.stationCmd.carMode);
-    }
-
-    ServiceCarStatus.stationCmd.has_emergencyStop=v2x_message.has_emergencystop();
-    if(ServiceCarStatus.stationCmd.has_emergencyStop)
-    {
-            ServiceCarStatus.stationCmd.emergencyStop=v2x_message.emergencystop();
-
-            qDebug("ServiceCarStatus.stationCmd.emergencyStop:",ServiceCarStatus.stationCmd.emergencyStop);
-            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.emergencyStop:%d",ServiceCarStatus.stationCmd.emergencyStop);
-    }
-
-    ServiceCarStatus.stationCmd.has_stationStop=v2x_message.has_stationstop();
-    if(ServiceCarStatus.stationCmd.has_stationStop)
-    {
-            ServiceCarStatus.stationCmd.stationStop=v2x_message.stationstop();
-
-            qDebug("ServiceCarStatus.stationCmd.stationStop:",ServiceCarStatus.stationCmd.stationStop);
-            givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.stationStop:%d",ServiceCarStatus.stationCmd.stationStop);
-    }
-    int num=v2x_message.stationid_size();
-    if(num>0)
-    {
-        ServiceCarStatus.stationCmd.stationTotalNum=num;
-        for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
-        {
-            ServiceCarStatus.stationCmd.stationGps[i].gps_lat=v2x_message.stgps(i).lat();
-            ServiceCarStatus.stationCmd.stationGps[i].gps_lng=v2x_message.stgps(i).lon();
-
-            qDebug("stationGps: %d, lat: %.7f, lon: %.7f", v2x_message.stationid(i), ServiceCarStatus.stationCmd.stationGps[i].gps_lat, ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
-            givlog->debug("brain_v2x","stationGps: %d, lat: %.7f, lon: %.7f",
-                           v2x_message.stationid(i),ServiceCarStatus.stationCmd.stationGps[i].gps_lat,
-                          ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
-        }
-        ServiceCarStatus.mbRunPause=false;
-    }
-
-}
-
-void iv::decition::BrainDecition::Updateultra(const char *pdata, const int ndatasize)
-{
-    iv::ultrasonic::ultrasonic ultraarry;
-    if(false == ultraarry.ParseFromArray(pdata,ndatasize))
-    {
-        std::cout<<"Listenultra fail."<<std::endl;
-        return;
-    }
-
-    ServiceCarStatus.ultraDistance[1]=ultraarry.sigobjdist_flcorner();
-    ServiceCarStatus.ultraDistance[2]=ultraarry.sigobjdist_flmiddle();
-    ServiceCarStatus.ultraDistance[3]=ultraarry.sigobjdist_flside();
-    ServiceCarStatus.ultraDistance[4]=ultraarry.sigobjdist_frcorner();
-    ServiceCarStatus.ultraDistance[5]=ultraarry.sigobjdist_frmiddle();
-    ServiceCarStatus.ultraDistance[6]=ultraarry.sigobjdist_frside();
-    ServiceCarStatus.ultraDistance[7]=ultraarry.sigobjdist_rlcorner();
-    ServiceCarStatus.ultraDistance[8]=ultraarry.sigobjdist_rlmiddle();
-    ServiceCarStatus.ultraDistance[9]=ultraarry.sigobjdist_rlside();
-    ServiceCarStatus.ultraDistance[10]=ultraarry.sigobjdist_rrcorner();
-    ServiceCarStatus.ultraDistance[11]=ultraarry.sigobjdist_rrmiddle();
-    ServiceCarStatus.ultraDistance[12]=ultraarry.sigobjdist_rrside();
-}
-
-
-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/decision/brain.h

@@ -1,176 +0,0 @@
-#pragma once
-/*
-* 中央决策大脑
-*/
-
-#ifndef _IV_DECITION_BRAIN_
-#define _IV_DECITION_BRAIN_
-
-#include <common/boost.h>
-#include <common/gps_type.h>
-//#include <control/controller.h>
-//#include <control/can.h>
-#include <perception/eyes.h>
-#include <decision/decition_maker.h>
-//#include <decition/decition_executer.h>
-#include <decision/decition_voter.h>
-#include <decision/decide_gps_00.h>
-#include <decision/decide_line_00.h>
-#include <decision/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 "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 "v2x.pb.h"
-#include "ultrasonic.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::StationCmd   stationCmd;
-            int lastMode;
-            bool lastPause;
-
-			void run();	//实际执行逻辑
-
-            void * mpamapreq;
-            void * mpa;
-            void * mpvbox;
-            QMutex mMutexMap;
-
-            void * mpaDecition;
-            void * mpaVechicleState;
-            void * mpaToPlatform;
-            void * mpaPlanTrace;
-            void * mpaObsTraceLeft;
-            void * mpaObsTraceRight;
-
-
-            void ShareDecition(iv::decition::Decition decition);
-            void ShareBrainstate(iv::brain::brainstate xbs);
-
-        private:
-            void * mpaPAD;
-            void * mpaHMI;
-            void * mpaPlatform;
-            void *mpaGroup;
-            void * mpmapChangeSig;
-            void * v2x;
-            void * ultra;
-            std::string mstrmemmap_index;
-
-            int mnOldTime;
-            QTime mTime;
-            double mfSpeedLast;
-            double mfVehSpeedLast;
-
-
-        public:
-            void UpdatePAD(const char *pdata, const int ndatasize);
-            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 Updatev2x(const char * pdata,const int ndatasize);
-            void Updateultra(const char * pdata,const int ndatasize);
-
-        private:
-//            Adcivstatedb * mpasd;
-            void UpdateChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-            void adjuseGpsLidarPosition();
-
-            fanyaapi mmpcapi;
-
-            bool mbUseExternMPC = false;
-        };
-
-    }
-}
-
-#endif // !_IV_DECITION_BRAIN_

src/decition/decition_brain/decision/compute_00.cpp

@@ -1,1843 +0,0 @@
-#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/decision/compute_00.h

@@ -1,96 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_COMPUTE_00_
-#define _IV_DECITION_COMPUTE_00_
-
-#include <common/gps_type.h>
-#include <common/obstacle_type.h>
-#include <decision/decition_type.h>
-#include <vector>
-#include <decision/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/decision/decide_from_gps.cpp

@@ -1,605 +0,0 @@
-#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/decision/decide_gps_00.cpp

@@ -1,3923 +0,0 @@
-#include <decision/decide_gps_00.h>
-#include <decision/adc_tools/compute_00.h>
-#include <decision/adc_tools/gps_distance.h>
-#include <decision/decition_type.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/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 <decision/adc_controller/base_controller.h>
-#include <decision/adc_controller/pid_controller.h>
-#include <decision/adc_planner/lane_change_planner.h>
-#include <decision/adc_planner/frenet_planner.h>
-#include <QTime>
-
-using namespace std;
-
-#include "ivlog.h"
-extern iv::Ivlog * givlog;
-
-#define PI (3.1415926535897932384626433832795)
-
-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;
-
-
-double avoidX =0;
-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;   //标志是否用老方法避障
-
-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;
-
-
-std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
-std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear;
-
-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);
-
-bool qiedianCount = false;
-//日常展示
-
-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;
-
-
-    //如果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
-        }
-
-        //	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;
-        isFirstRun = false;
-    }
-
-
-    if(ServiceCarStatus.msysparam.gpsOffset_x!=0 || ServiceCarStatus.msysparam.gpsOffset_y!=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;
-
-
-
-
-
-
-
-
-
-
-
-    gpsTraceOri.clear();
-    gpsTraceNow.clear();
-    gpsTraceAim.clear();
-    gpsTraceAvoid.clear();
-    gpsTraceBack.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;
-
-
-
-
-
-
-
-
-    //2020-01-03, kkcai
-    //if(!circleMode && PathPoint>gpsMapLine.size()-200){
-    if(!circleMode && PathPoint>gpsMapLine.size()-100){
-        minDecelerate=-1.0;
-        std::cout<<"map finish brake"<<std::endl;
-    }
-
-
-
-    if(!ServiceCarStatus.inRoadAvoid){
-        roadOri = gpsMapLine[PathPoint]->roadOri;
-        roadSum = gpsMapLine[PathPoint]->roadSum;
-    }else{
-        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
-        roadSum = gpsMapLine[PathPoint]->roadSum*3;
-    }
-
-    if(ServiceCarStatus.avoidObs){
-         gpsMapLine[PathPoint]->runMode=1;
-    }else{
-         gpsMapLine[PathPoint]->runMode=0;
-    }
-
-    if(roadNowStart){
-        roadNow=roadOri;
-        roadNowStart=false;
-    }
-
- //   avoidX = (roadOri-roadNow)*roadWidth;
-    avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
-
-
-    if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
-                                                ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
-    {
-        vehState = normalRun;
-        roadNow = roadOri;
-    }
-
-
-
-    //    if (PathPoint!=-1&&((now_gps_ins.rtk_status==6)||(now_gps_ins.rtk_status==5))&&GetDistance(now_gps_ins,*gpsMapLine[PathPoint])<30)
-    //    {
-    //        DecideGps00::lastGpsIndex = PathPoint;
-    //        gpsMissCount = 0;
-
-    //    }
-    //    else
-    //    {
-    //        gpsMissCount++;
-    //    }
-
-    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
-    //    gpsTraceOri = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
-    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;
-            }
-        }
-    }
-
-    if(gpsTraceNow.size()==0){
-        if (roadNow==roadOri)
-        {
-            gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
-            //gpsTraceNow = getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex);
-        }else
-        {
-            //	gpsTraceNow = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-roadNow))], lastGpsIndex);
-            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-            //gpsTraceNow = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
-        }
-    }
-
-
-    //  dSpeed = getSpeed(gpsTraceNow);
-    dSpeed = 80;
-
-    //   planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26
-
-    planTrace.clear();
-    for(int i=0;i<gpsTraceNow.size();i++){
-        TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
-        planTrace.push_back(pt);
-    }
-
-
-    dSpeed = limitSpeed(controlAng, dSpeed);
-
-
-
-    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
-    if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
-        controlAng=0;
-    }
-
-
-
-    //1220
-    if(ServiceCarStatus.daocheMode){
-        controlAng=0-controlAng;
-    }
-
-    //2020-0106
-    if(vehState==avoiding){
-        controlAng=max(-150.0,controlAng);
-        controlAng=min(150.0,controlAng);
-    }
-    if(vehState==backOri){
-        controlAng=max(-120.0,controlAng);
-        controlAng=min(120.0,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;
-
-                //1125
-                //                 gps_decition->brake=20;
-                //                 gps_decition->accelerator = -3;
-                //                 gps_decition->wheel_angle = 0-controlAng;
-                //                 gps_decition->speed = 0;
-                //                 gps_decition->torque=0;
-                //                 return gps_decition;
-
-
-
-
-
-            }
-
-        }
-
-    }
-
-
-
-
-
-    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 >= 12.5 && parkDis<20)
-            {
-                dSpeed = min(dSpeed, 8.0);
-            }else if (parkDistance < 12.5 && parkDistance >= 8.5 && parkDis<15.0){
-                dSpeed = min(dSpeed, 5.0);
-            }else if (parkDistance < 8.5 && parkDistance >= 5.5 && parkDis<9.0){
-                dSpeed = min(dSpeed, 3.0);
-            }else if(parkDistance < 5.5 && parkDis<6.0){
-                dSpeed = min(dSpeed, 1.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 (gpsMapLine[PathPoint]->roadMode ==0)
-    {
-        //dSpeed = min(10.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = false;
-
-    }else  if (gpsMapLine[PathPoint]->roadMode == 5)
-    {
-        //dSpeed = min(8.0,dSpeed);
-
-        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);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = false;
-
-    }else if (gpsMapLine[PathPoint]->roadMode == 14)
-    {
-        //dSpeed = min(8.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
-        gps_decition->leftlamp = true;
-        gps_decition->rightlamp = false;
-    }else if (gpsMapLine[PathPoint]->roadMode == 15)
-    {
-        //dSpeed = min(8.0,dSpeed);
-
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = true;
-    }else if (gpsMapLine[PathPoint]->roadMode == 16)
-    {
-        // dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
-        //gps_decition->leftlamp = true;
-        //gps_decition->rightlamp = false;
-    }else if (gpsMapLine[PathPoint]->roadMode == 17)
-    {
-        //  dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
-        //gps_decition->leftlamp = false;
-        //gps_decition->rightlamp = true;
-    }else if (gpsMapLine[PathPoint]->roadMode == 18)
-    {
-        // dSpeed = min(10.0,dSpeed);
-        dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
-        gps_decition->leftlamp = false;
-        gps_decition->rightlamp = false;
-
-        /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
-        {
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }
-        else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
-        {
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }*/
-    }else if (gpsMapLine[PathPoint]->roadMode == 1)
-    {
-        dSpeed = min(10.0,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);
-
-    }
-
-    if((gpsMapLine[PathPoint]->speed)>0.001)
-    {
-        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
-    }
-
-
-    dSecSpeed = dSpeed / 3.6;
-
-
-
-
-
-
-    std::cout<<"juecesudu2="<<dSpeed<<std::endl;
-
-
-
-
-
-
-
-    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;
-        }
-    }
-
-    //    qDebug("decide0 time is %d",xTime.elapsed());
-
-    //1220
-    if(!ServiceCarStatus.daocheMode){
-        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-    }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
-    }
-
-
-
-
-    //old_bz--------------------------------------------------------------------------------------------------
-
-    if (vehState == avoiding)
-    {
-        cout<<"\n==================avoiding=================\n"<<endl;
-        //  vehState=normalRun; //1025
-        if (dSpeed > 10) {
-            dSpeed = 10;
-        }
-        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
-        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
-            vehState = normalRun;
-            //            useFrenet = false;
-            //            useOldAvoid = true;
-        }
-        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
-            // vehState = avoidObs; 0929
-            vehState = normalRun;
-        }
-        else if (gpsTraceNow[0].x>0)
-        {
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }
-        else if(gpsTraceNow[0].x<0)
-        {
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }
-    }
-
-
-    if (vehState==preBack)
-    {
-        vehState = backOri;
-    }
-
-
-    if (vehState==backOri)
-    {
-        cout<<"\n================backOri===========\n"<<endl;
-        //  vehState=normalRun; //1025
-        if (dSpeed > 10) {
-            dSpeed = 10;
-        }
-
-        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
-        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
-            vehState = normalRun;
-            //            useFrenet = false;
-            //            useOldAvoid = true;
-        }
-        else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
-            // vehState = avoidObs; 0929
-            vehState = normalRun;
-        }
-        else if (gpsTraceNow[0].x>0)
-        {
-            gps_decition->leftlamp = false;
-            gps_decition->rightlamp = true;
-        }
-        else if (gpsTraceNow[0].x<0)
-        {
-            gps_decition->leftlamp = true;
-            gps_decition->rightlamp = false;
-        }
-    }
-
-    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
-
-    cout<<"\n当前RoadNum:%d\n"<<roadNow<<endl;
-
-
-
-
-
-
-
-
-
-    //   计算回归原始路线
-
-    if (roadNow!=roadOri)
-    {
-        //        useFrenet = true;
-        //        useOldAvoid = false;
-
-        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);
-        gpsTraceNow.clear();
-        if(useOldAvoid){
-            gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-        }
-        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;
-
-    }
-
-    //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.socfDis+20.0) &&(abs(realspeed)<1.0) &&
-            (vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
-            && (gpsMapLine[PathPoint]->runMode==1)){
-        ObsTimeStart=GetTickCount();
-        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;
-        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
-    }
-
-    if(obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis){
-        ObsTimeStart=-1;
-        ObsTimeEnd=-1;
-        ObsTimeSpan=-1;
-
-    }
-
-
-    //避障模式
-
-
-    if (vehState == avoidObs   || vehState==waitAvoid ) {
-
-        //      if (obsDistance <20 && obsDistance >0)
-        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
-        {
-            dSpeed = min(7.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;
-        }
-
-    }
-
-
-    if (vehState == preAvoid)
-    {
-        cout<<"\n====================preAvoid==================\n"<<endl;
-        /* if (obsDisVector[roadNow]>30)*/  //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
-        if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
-        {
-            // vehState = avoidObs; 0929
-            vehState=normalRun;
-        }
-        else
-        {
-            //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
-            if(useOldAvoid){
-                roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
-              //  avoidX = (roadOri - roadNow)*roadWidth;  //1012
-                 avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
-            }
-            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);
-            }
-
-            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);
-                    gpsTraceNow.clear();
-                    gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
-                }
-                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;
-
-                hasCheckedAvoidLidar = false;
-
-                cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;
-
-            }
-        }
-    }
-
-    //--------------------------------------------------------------------------------old_bz_end
-
-
-
-
-
-    //TOUCHEPINGBI
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-    if (parkMode)
-    {
-        minDecelerate=-0.4;
-
-        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
-            parkMode=false;
-        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>4){
-            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 (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.stationCmd.received==true)
-    {
-        std::vector<Station>  station_received;
-        Station station_aa,station_nearest;
-
-        if(ServiceCarStatus.stationCmd.has_emergencyStop)
-        {
-            if(ServiceCarStatus.stationCmd.emergencyStop==0x01)
-            {
-                //ServiceCarStatus.carState = 0;
-                //busMode=true;
-                ServiceCarStatus.emergencyStop=1;
-            }
-            else
-            {
-                //ServiceCarStatus.carState = 1;
-                //busMode=false;
-                ServiceCarStatus.emergencyStop=0;
-            }
-        }
-
-        if(ServiceCarStatus.stationCmd.has_stationStop)
-        {
-            //寻找最近站点
-            station_received.clear();
-            for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
-            {
-                station_aa.index=i;
-                station_aa.station_location.gps_lat=ServiceCarStatus.stationCmd.stationGps[i].gps_lat;
-                station_aa.station_location.gps_lng=ServiceCarStatus.stationCmd.stationGps[i].gps_lng;
-                GaussProjCal(station_aa.station_location.gps_lng,station_aa.station_location.gps_lat, &station_aa.station_location.gps_x, &station_aa.station_location.gps_y);
-                station_received.push_back(station_aa);
-            }
-            station_nearest=getNearestStation(now_gps_ins,station_received,gpsMapLine);
-
-            qDebug("station_nearest: %d, lat: %.7f, lon: %.7f", station_nearest.map_index, station_nearest.station_location.gps_lat, ServiceCarStatus.amilng=station_nearest.station_location.gps_lng);
-            givlog->debug("brain_v2x","station_nearest: %d, lat: %.7f, lon: %.7f",
-                          station_nearest.map_index, station_nearest.station_location.gps_lat,
-                          station_nearest.station_location.gps_lng);
-            //进入站点模式
-            if((ServiceCarStatus.stationCmd.stationStop==0x00))
-            {
-                ServiceCarStatus.carState = 2;
-                ServiceCarStatus.amilat=station_nearest.station_location.gps_lat;
-                ServiceCarStatus.amilng=station_nearest.station_location.gps_lng;
-                busMode=true;
-            }else
-            {
-                ServiceCarStatus.carState = 1;
-                busMode=false;
-                ServiceCarStatus.station_control_door=0;
-                ServiceCarStatus.station_control_door_option=false;
-            }
-        }
-        if(ServiceCarStatus.stationCmd.has_carMode)
-        {
-            if(ServiceCarStatus.stationCmd.carMode==0x00)
-            {
-                ServiceCarStatus.stationCmd.mode_manual_drive=true;
-            }else
-            {
-                ServiceCarStatus.stationCmd.mode_manual_drive=false;
-            }
-        }
-
-        ServiceCarStatus.stationCmd.received=false;
-    }
-
-
-
-
-    //carState == 0，紧急停车
-    if ((ServiceCarStatus.emergencyStop==1))  //||(adjuseultra()==true))
-    {
-        minDecelerate=-1.0;
-    }
-
-    if (ServiceCarStatus.carState == 3 && busMode)
-    {
-
-        if(realspeed<0.1){
-            ServiceCarStatus.carState=0;
-            minDecelerate=-1.0;
-        }else{
-            nearStation=true;
-            dSpeed=0;
-        }
-
-    }
-
-    //carState==2,站点停车
-    if ((ServiceCarStatus.carState==2)&&busMode)
-    {
-
-        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
-        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
-
-        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-        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);
-
-        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);
-        }
-
-        if((pt.y<5)&&(abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
-        {
-            ServiceCarStatus.station_control_door=1;   //open door
-        }
-
-        //站点停车log
-        givlog->debug("brain_v2x","aim_gps_ins.gps_lat: %.7f, aim_gps_ins.gps_lng: %.7f, dis: %.2f, dSpeed: %.1f",
-                      aim_gps_ins.gps_lat, aim_gps_ins.gps_lng,
-                      dis,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")&&(abs(realspeed)<18)){
-        if(obsDistance>0 && obsDistance<6){
-            dSpeed=0;
-        }
-    }else if(obsDistance>0 && obsDistance<10){
-        dSpeed=0;
-    }
-
-    //  obsDistance=-1; //1227
-
-    if(ServiceCarStatus.group_control){
-        dSpeed = ServiceCarStatus.group_comm_speed;
-    }
-    if(dSpeed==0){
-        minDecelerate=min(-1.0f,minDecelerate);
-    }
-
-    std::cout<<"dSpeed= "<<dSpeed<<std::endl;
-
-    // givlog->debug("SPEED","dSpeed is %f",dSpeed);
-    gps_decition->wheel_angle = 0.0 - controlAng;
-    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
-
-
-
-    lastAngle=gps_decition->wheel_angle;
-
-    //   qDebug("decide1 time is %d",xTime.elapsed());
-
-    for(int i=0;i<ServiceCarStatus.obsTraceLeft.size();i++){
-        givlog->debug("obs_distance","obsTraceLeftX: %f, obsTraceLeftY: %f",
-                      ServiceCarStatus.obsTraceLeft.at(i).x, ServiceCarStatus.obsTraceLeft.at(i).y);
-    }
-
-
-    return gps_decition;
-}
-
-iv::Station iv::decition::DecideGps00::getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap){
-
-    int now_index=0,front_index=0;
-    int station_size=station_n.size();
-
-    for(int i=0;i<station_size;i++)
-    {
-        int minDistance=10;
-        for (int j = 0; j < gpsMap.size(); j++)
-        {
-            double tmpdis = GetDistance(station_n[i].station_location, (*gpsMap[j]));
-            if (tmpdis < minDistance )
-            {
-                minDistance = tmpdis;
-                station_n[i].map_index=j;
-            }
-        }
-        givlog->debug("brain_v2x","stationi: %d, map_index: %d",
-                      i,station_n[i].map_index);
-
-    }
-    int minDistance=10;
-    for (int j = 0; j < gpsMap.size(); j++)
-    {
-        double tmpdis = GetDistance(now_gps_ins, (*gpsMap[j]));
-        if (tmpdis < minDistance )
-        {
-            minDistance = tmpdis;
-            now_index=j;
-        }
-    }
-
-    givlog->debug("brain_v2x","now_index: %d",
-                  now_index);
-
-    int min_index=gpsMap.size()-1;
-    int station_index=0;
-    for(int i=0;i<station_size;i++)
-    {
-        if(station_n[i].map_index>now_index)
-        {
-            front_index=station_n[i].map_index;
-            if(front_index<min_index)
-            {
-                min_index=front_index;
-                station_index=i;
-            }
-        }
-    }
-
-    qDebug("station_index:%d",station_index);
-
-    return station_n[station_index];
-
-}
-
-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();
-    yuhesen_Adapter = new YuHeSenAdapter();
-
-
-    mpid_Controller.reset(pid_Controller);
-    mbus_Adapter.reset(bus_Adapter);
-    mhapo_Adapter.reset(hapo_Adapter);
-    myuhesen_Adapter.reset(yuhesen_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.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=="yuhesen"){
-        yuhesen_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;
-    //	int  PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
-    /*if (PathPoint != -1)
-        lastGpsIndex = PathPoint;*/
-
-    if (gpsMapLine.size() > 600 && PathPoint >= 0) {
-        int aimIndex;
-        if(circleMode){
-            aimIndex=PathPoint+600;
-        }else{
-            aimIndex=min((PathPoint+600),(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;
-            }
-
-            //			if (pt.v1 == 22 || pt.v1 == 23)
-            //			{
-            //				readyParkMode = true;
-            //				DecideGps00::finishPointNum = i;
-            //			}
-
-
-            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);
-        }
-    }
-    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;
-}
-
-
-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;
-    }
-
-
-    // obsPredict start
-    if(ServiceCarStatus.useObsPredict){
-        float preObsDis=200;
-        if(!lidar_per.empty()){
-            preObsDis=PredictObsDistance(  secSpeed,  gpsTrace, lidar_per);
-            lastPreObsDistance=preObsDis;
-        }else{
-            preObsDis=lastPreObsDistance;
-        }
-        if(preObsDis<lidarDistance || lidarDistance==-1){
-            lidarDistance=preObsDis;
-        }
-    }
-    // obsPredict end
-
-    //    qDebug("time 1 is %d ",xTime.elapsed());
-    //    if(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
-    //        lidarDistance=-1;
-    //    }
-
-    getEsrObsDistance(gpsTrace, roadNum);
-
-    double mobieye_ttc,mobieye_obsid,mobieye_speed,mobieye_distance;
-
-    double fveh_width = 2.0;
-#ifdef USE_MOBIEYE_OBS
-    MobieyeInst.GetObsFromMobieye(&gpsTrace,mobieye_distance,mobieye_speed,mobieye_ttc,mobieye_obsid,fveh_width);
-#endif
-    //   qDebug("time 2 is %d ",xTime.elapsed());
-    //  esrDistance=-1;
-
-    //    if(PathPoint>2992 && PathPoint<4134){
-    //        lidarDistance=-1;
-    //    }
-
-
-    //    if(PathPoint>10193 && PathPoint<10929){
-    //        esrDistance=-1;
-    //    }
-
-
-    if(lidarDistance<0){
-        lidarDistance=500;
-    }
-
-#ifdef USE_MOBIEYE_OBS
-    esrDistance = mobieye_distance;
-
-    if(esrDistance>150){
-        esrDistance=500;
-    }
-#else
-    if(esrDistance>30){
-        esrDistance=500;
-    }
-#endif
-
-
-
-    if(esrDistance<0){
-        esrDistance=500;
-    }
-
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
-
-
-    //        if(esrDistance>30){
-    //            esrDistance=-1;
-    //        }
-
-
-
-    //            if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
-    //                if(abs(lidarDistance-esrDistance)>5){
-
-    //                    esrDistance=-1;
-    //                }
-
-    //            }
-
-
-
-    //            if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
-    //                && gpsMapLine[PathPoint]->runMode == 1    ){
-    //                if(abs(lidarDistance-esrDistance)>5){
-
-    //                    esrDistance=-1;
-    //                }
-
-    //            }
-
-
-
-    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)
-        {
-#ifdef USE_MOBIEYE_OBS
-            obs = esrDistance;
-            obsSd = mobieye_speed;
-#else
-            //	obsDistance = esrDistance;
-            obs = esrDistance;
-            //	obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
-#endif
-        }
-        else if (!ServiceCarStatus.obs_radar.empty())
-        {
-            //	obsDistance = lidarDistance;
-            obs = lidarDistance;
-            //	obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
-            obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
-            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
-        }
-        else
-        {
-            //	obsDistance = lidarDistance;
-            obs=lidarDistance;
-            //	obsSpeed = 0 - secSpeed;
-            obsSd = 0 -secSpeed;
-            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
-        }
-    }
-    else if (esrDistance>0)
-    {
-        //		obsDistance = esrDistance;
-        //		obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-
-#ifdef USE_MOBIEYE_OBS
-        obs = esrDistance;
-        obsSd = mobieye_speed;
-#else
-        obs = esrDistance;
-        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-#endif
-    }
-    else if (lidarDistance > 0)
-    {
-        //		obsDistance = lidarDistance;
-        //		obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
-        obs = lidarDistance;
-        obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
-        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
-    }
-    else {
-        //		obsDistance = esrDistance;
-        //		obsSpeed = 0 - secSpeed;
-        obs = esrDistance;
-        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;
-    }
-
-    givlog->debug("obs_distance","obs: %f, lidar_obs: %f, Radar_obs: %f, lidar_obs_x: %f, lidar_obs_y: %f",
-                  ServiceCarStatus.mObs, ServiceCarStatus.mLidarObs,
-                  ServiceCarStatus.mRadarObs,obsPoint.x,lidarDistance);
-
-    //   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(!ServiceCarStatus.obs_rear_radar.empty()){
-        getRearEsrObsDistance(gpsTrace, roadNum);
-    }
-    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(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
-    //        lidarDistance=-1;
-    //    }
-
-    //  getEsrObsDistance(gpsTrace, roadNum);
-    esrDistance=-1;
-
-
-    //    if(PathPoint>2992 && PathPoint<4134){
-    //        lidarDistance=-1;
-    //    }
-
-
-    //    if(PathPoint>10193 && PathPoint<10929){
-    //        esrDistance=-1;
-    //    }
-
-
-    if(lidarDistance<0){
-        lidarDistance=500;
-    }
-    if(esrDistance<0){
-        esrDistance=500;
-    }
-
-
-    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
-
-
-    //        if(esrDistance>30){
-    //            esrDistance=-1;
-    //        }
-
-
-
-    //            if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
-    //                if(abs(lidarDistance-esrDistance)>5){
-
-    //                    esrDistance=-1;
-    //                }
-
-    //            }
-
-
-
-    //            if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
-    //                && gpsMapLine[PathPoint]->runMode == 1    ){
-    //                if(abs(lidarDistance-esrDistance)>5){
-
-    //                    esrDistance=-1;
-    //                }
-
-    //            }
-
-
-
-    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)
-        {
-            //	obsDistance = esrDistance;
-            obs = esrDistance;
-            //	obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
-        }
-        else if (!ServiceCarStatus.obs_radar.empty())
-        {
-            //	obsDistance = lidarDistance;
-            obs = lidarDistance;
-            //	obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
-            obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
-            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
-        }
-        else
-        {
-            //	obsDistance = lidarDistance;
-            obs=lidarDistance;
-            //	obsSpeed = 0 - secSpeed;
-            obsSd = 0 -secSpeed;
-            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
-        }
-    }
-    else if (esrDistance>0)
-    {
-        //		obsDistance = esrDistance;
-        //		obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-        obs = esrDistance;
-        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
-    }
-    else if (lidarDistance > 0)
-    {
-        //		obsDistance = lidarDistance;
-        //		obsSpeed = Compute00().getObsSpeed(obsPoint, secSpeed);
-        obs = lidarDistance;
-        obsSd = Compute00().getObsSpeed(obsPoint, secSpeed);
-        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
-    }
-    else {
-        //		obsDistance = esrDistance;
-        //		obsSpeed = 0 - secSpeed;
-        obs = esrDistance;
-        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;
-    }
-
-}
-
-
-
-void iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
-    double preObsDis;
-
-    if(!lidar_per.empty()){
-        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
-        lastPreObsDistance=preObsDis;
-
-    }else{
-        preObsDis=lastPreObsDistance;
-    }
-    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;
-
-    //    if (roadNow<roadOri)
-    //    {
-    //        for (int i = 0; i < roadNow; i++)
-    //        {
-    //            gpsTraceAvoid.clear();
-    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-    //            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-    //        }
-
-
-    //        for (int i = roadOri+1; i < roadSum; i++)
-    //        {
-    //            gpsTraceAvoid.clear();
-    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            //	bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-    //            computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-    //        }
-    //    }
-    //    else if (roadNow>roadOri)
-    //    {
-    //        for (int i = 0; i < roadOri; i++)
-    //        {
-    //            gpsTraceAvoid.clear();
-    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-    //           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-    //        }
-    //        for (int i = roadNow + 1; i < roadSum; i++)
-    //        {
-    //            gpsTraceAvoid.clear();
-    //            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-    //           computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
-    //        }
-
-    //    }
-
-    //    else
-    //    {
-    //        for (int i = 0; i < roadOri; i++)
-    //        {
-    //            gpsTraceAvoid.clear();
-    //            //	gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-    //           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-    //        }
-    //        for (int i = roadOri + 1; i < roadSum; i++)
-    //        {
-    //            gpsTraceAvoid.clear();
-    //            //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-    //           computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-    //        }
-
-    //    }
-
-    for (int i =  0; i < roadSum; i++)
-    {
-        gpsTraceAvoid.clear();
-        //		gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
-     //   avoidX = (roadWidth)*(roadOri - i);
-         avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
-        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
-        //	computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
-        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] + 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] + 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;
-
-    //   computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-
-
-
-    //    if (roadNow>=roadOri+1)
-    //    {
-    //        for (int i = roadOri+1; i < roadNow; i++)
-    //        {
-    //            gpsTraceBack.clear();
-    //            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-    //        }
-    //    }
-    //    else if (roadNow <= roadOri - 1) {
-
-    //        for (int i = roadOri - 1; i > roadNow; i--)
-    //        {
-    //            gpsTraceBack.clear();
-    //            //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-    //            avoidX = (roadWidth)*(roadOri - i);
-    //            gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
-    //            computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-    //        }
-
-    //    }
-
-
-
-    for (int i =  0; i <roadSum; i++)
-    {
-        gpsTraceBack.clear();
-        //	gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
-     //   avoidX = (roadWidth)*(roadOri - i);
-         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, startAvoid_gps_ins)>avoidRunDistance)&&
-    //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
-    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.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;
-}
-
-
-
-bool iv::decition::DecideGps00::adjuseultra(){
-    bool front=false,back=false,left=false,right=false;
-    for(int i=1;i<=13;i++)
-    {
-        if((i==2)||(i==3)||(i==4)||(i==5))   //front
-        {
-            if(ServiceCarStatus.ultraDistance[i]<100)
-            {
-                front=true;
-            }
-        }else if((i==1)||(i==12)||(i==6)||(i==7))   //left,right
-        {
-            if(ServiceCarStatus.ultraDistance[i]<30)
-            {
-                left=true;
-            }
-        }else if((i==8)||(i==9)||(i==10)||(i==11))   //back
-        {
-            if(ServiceCarStatus.ultraDistance[i]<10)
-            {
-                back=true;
-            }
-        }
-    }
-
-    if(front||left||back)
-        return true;
-    else
-        return false;
-
-}
-
-void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
-    if(  gpsMapLine[PathPoint]->mode2==3000){
-        if(obsDistance>5){
-            obsDistance=200;
-        }
-        dSpeed=min(dSpeed,5.0);
-    }
-}
-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->mfRoadWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
-    float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
-    if(!ServiceCarStatus.inRoadAvoid){
-        x= (roadOri-roadAim)*gps->mfRoadWidth;
-    }else{
-        int num=roadOri-roadAim;
-        switch (abs(num%3)) {
-        case 0:
-            x=(num/3)*gps->mfRoadWidth;
-            break;
-        case 1:
-            if(num>0){
-                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide;
-            }else{
-                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide;
-            }
-            break;
-        case 2:
-            if(num>0){
-                x=(num/3)*gps->mfRoadWidth +veh_to_roadSide+roadSide_to_roadSide;
-            }else{
-                x=(num/3)*gps->mfRoadWidth -veh_to_roadSide-roadSide_to_roadSide;
-            }
-
-            break;
-        default:
-            break;
-        }
-    }
-
-    return x;
-}

src/decition/decition_brain/decision/decide_gps_00.h

@@ -1,261 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION__DECIDE_GPS_00_
-#define _IV_DECITION__DECIDE_GPS_00_
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_planner/frenet_planner.h>
-#include <decision/adc_controller/pid_controller.h>
-#include <decision/adc_adapter/ge3_adapter.h>
-#include <decision/adc_adapter/qingyuan_adapter.h>
-#include <decision/adc_adapter/vv7_adapter.h>
-#include <decision/adc_adapter/zhongche_adapter.h>
-#include <decision/adc_adapter/bus_adapter.h>
-#include <decision/adc_adapter/hapo_adapter.h>
-#include <decision/adc_adapter/zhongche_adapter.h>
-#include <decision/adc_adapter/yuhesen_adapter.h>
-#include "perception/perceptionoutput.h"
-#include "ivlog.h"
-#include <memory>
-
-#include "obs_mobieye.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;
-
-    ///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;
-
-    BaseController *pid_Controller;
-    BaseAdapter *ge3_Adapter;
-    BaseAdapter *qingyuan_Adapter;
-    BaseAdapter *vv7_Adapter;
-    BaseAdapter *zhongche_Adapter;
-    BaseAdapter *bus_Adapter;
-    BaseAdapter *hapo_Adapter;
-    BaseAdapter *yuhesen_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> myuhesen_Adapter;
-
-	FrenetPlanner *frenetPlanner;
-//    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 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);
-    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);
-
-    void 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<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);
-
-    float computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth);
-
-    bool adjuseultra();
-
-    iv::Station getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap);
-
-    void transferGpsMode2( const std::vector<GPSData> gpsMapLine);
-    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=1500;
-    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 bool parkBesideRoad;
-extern double steerSpeed;
-extern bool transferPieriod;
-extern bool transferPieriod2;
-extern double traceDevi;
-#endif // !  _IV_DECITION__DECIDE_GPS_00_
-
-

src/decition/decition_brain/decision/decide_line_00.h

@@ -1,252 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION__DECIDE_LINE_00_
-#define _IV_DECITION__DECIDE_LINE_00_
-
-#include <common/gps_type.h>
-#include <decision/decition_type.h>
-#include <common/obstacle_type.h>
-#include <vector>
-#include <decision/gnss_coordinate_convert.h>
-#include <decision/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/decision/decide_line_00_.cpp

@@ -1,979 +0,0 @@
-#include <decision/decide_line_00.h>
-#include <decision/compute_00.h>
-#include <decision/gps_distance.h>
-#include <decision/decition_type.h>
-#include <decision/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/decision/decision.pri

@@ -1,56 +0,0 @@
-HEADERS += \
-    $$PWD/obs_mobieye.h   \
-    $$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/yuhesen_adapter.h
-
-SOURCES += \
-    $$PWD/decide_gps_00.cpp \
-    $$PWD/brain.cpp \
-    $$PWD/decide_line_00_.cpp \
-    $$PWD/obs_mobieye.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_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/zhongche_adapter.cpp \
-    $$PWD/adc_adapter/yuhesen_adapter.cpp

src/decition/decition_brain/decision/decition_maker.h

@@ -1,62 +0,0 @@
-#pragma once
-#ifndef _IV_DECITION_DECITION_MAKER_
-#define _IV_DECITION_DECITION_MAKER_
-
-#include <common/gps_type.h>
-#include <decision/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/decision/decition_type.h

@@ -1,61 +0,0 @@
-#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;
-    bool 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 ;                  //空调风力
-    float   roof_light ;                  //顶灯
-    float   home_light ;                  //日光灯
-    float   air_worktime ;                  //空调工作时间
-    float   air_offtime ;                  //空调关闭时间
-
-};
-typedef boost::shared_ptr<DecitionBasic> Decition;	//决策
-}
-}
-#endif // !_IV_DECITION_DECITION_TYPE_
-

src/decition/decition_brain/decision/decition_voter.cpp

@@ -1,22 +0,0 @@
-#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/decision/decition_voter.h

@@ -1,28 +0,0 @@
-#pragma once
-//对来自不同传感器所作出的决策进行加权判断  得出最终的决策（速度、角度)
-//障碍物信息-激光雷达
-//障碍物信息-毫米波雷达
-//障碍物信息-摄像头
-//局部地图规划出的路线
-//GPS 惯导和导航数据计算得出的路线
-#ifndef _IV_DECITION_VOTER_
-#define _IV_DECITION_VOTER_
-
-#include <decision/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/decision/fanyaapi.cpp

@@ -1,58 +0,0 @@
-#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/decision/fanyaapi.h

@@ -1,38 +0,0 @@
-#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/decision/gnss_coordinate_convert.cpp

@@ -1,153 +0,0 @@
-#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/decision/gnss_coordinate_convert.h

@@ -1,26 +0,0 @@
-#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/decision/gnss_proj/gnss_proj.cpp

@@ -1,179 +0,0 @@
-#include <cmath>
-#include <stdio.h>
-#include "gnss_proj.h"
-
-namespace iv {
-namespace localization {
-
-// 椭球体半长轴
-const static double WGS84_a = 6378137.0;
-// 第一离心率平方
-const static double WGS84_e2 = 0.006694380022903;
-// 第二离心率平方
-const static double WGS84_ep2 = WGS84_e2 / (1 - WGS84_e2);
-// 第一离心率高次方
-const static double WGS84_e4 = WGS84_e2 * WGS84_e2;
-const static double WGS84_e6 = WGS84_e2 * WGS84_e4;
-// 将经线长度作为纬度的函数进行 Taylor 展开时前四项的系数
-const static double WGS84_a1 = 1 - WGS84_e2 / 4 - (3 * WGS84_e4) / 64 - (5 * WGS84_e6) / 256;
-const static double WGS84_a2 = (3 * WGS84_e2) / 8 + (3 * WGS84_e4) / 32 + (45 * WGS84_e6) / 1024;
-const static double WGS84_a3 = (15 * WGS84_e4) / 256 + (45 * WGS84_e6) / 1024;
-const static double WGS84_a4 = (35 * WGS84_e6) / 3072;
-// UTM 展平因子，距离中心线 220km 的两经线会被近似拉伸为平行线
-const static double UTM_k0 = 0.9996;
-
-inline double deg2rad(double x) { return x * 0.017453292519943295; }
-
-inline double rad2deg(double x) { return x * 57.29577951308232; }
-
-
-int getUTMZoneNumber(double latitude, double longitude)
-{
-    longitude = longitude - int((longitude + 180) / 360) * 360;
-    int zone_number = int((longitude + 180) / 6) + 1;
-    if (latitude >= 56 && latitude < 64 && longitude >= 3 && longitude < 12)
-    {
-        zone_number = 32;
-    }
-
-    if (latitude >= 72 && latitude < 84)
-    {
-        if      (longitude >= 0  && longitude <  9) zone_number = 31;
-        else if (longitude >= 9  && longitude < 21) zone_number = 33;
-        else if (longitude >= 21 && longitude < 33) zone_number = 35;
-        else if (longitude >= 21 && longitude < 42) zone_number = 37;
-    }
-    return zone_number;
-}
-
-
-char getUTMLetterDesignator(double latitude)
-{
-    char LetterDesignator;
-
-    if     ((84 >= latitude) && (latitude >= 72))  LetterDesignator = 'X';
-    else if ((72 > latitude) && (latitude >= 64))  LetterDesignator = 'W';
-    else if ((64 > latitude) && (latitude >= 56))  LetterDesignator = 'V';
-    else if ((56 > latitude) && (latitude >= 48))  LetterDesignator = 'U';
-    else if ((48 > latitude) && (latitude >= 40))  LetterDesignator = 'T';
-    else if ((40 > latitude) && (latitude >= 32))  LetterDesignator = 'S';
-    else if ((32 > latitude) && (latitude >= 24))  LetterDesignator = 'R';
-    else if ((24 > latitude) && (latitude >= 16))  LetterDesignator = 'Q';
-    else if ((16 > latitude) && (latitude >= 8))   LetterDesignator = 'P';
-    else if (( 8 > latitude) && (latitude >= 0))   LetterDesignator = 'N';
-    else if (( 0 > latitude) && (latitude >= -8))  LetterDesignator = 'M';
-    else if ((-8 > latitude) && (latitude >= -16)) LetterDesignator = 'L';
-    else if((-16 > latitude) && (latitude >= -24)) LetterDesignator = 'K';
-    else if((-24 > latitude) && (latitude >= -32)) LetterDesignator = 'J';
-    else if((-32 > latitude) && (latitude >= -40)) LetterDesignator = 'H';
-    else if((-40 > latitude) && (latitude >= -48)) LetterDesignator = 'G';
-    else if((-48 > latitude) && (latitude >= -56)) LetterDesignator = 'F';
-    else if((-56 > latitude) && (latitude >= -64)) LetterDesignator = 'E';
-    else if((-64 > latitude) && (latitude >= -72)) LetterDesignator = 'D';
-    else if((-72 > latitude) && (latitude >= -80)) LetterDesignator = 'C';
-    else LetterDesignator = 'Z';
-    return LetterDesignator;
-}
-
-
-void getUTMZone(double latitude, double longitude, char* UTMZone)
-{
-    int zone_number = getUTMZoneNumber(latitude, longitude);
-    char letter = getUTMLetterDesignator(latitude);
-    snprintf(UTMZone, 4, "%d%c", zone_number, letter);
-}
-
-
-void latlon2NE(double latitude, double longitude,
-               double &northing, double &easting)
-{
-    double lat_rad = deg2rad(latitude);
-    double lon_rad = deg2rad(longitude);
-
-    // 经线在输入点处的曲率半径
-    double N = WGS84_a / sqrt(1 - WGS84_e2 * sin(lat_rad) * sin(lat_rad));
-    // 赤道沿经线到输入点的长度，使用 Taylor 展开逼近计算
-    double M = WGS84_a * (
-        WGS84_a1 * lat_rad -
-        WGS84_a2 * sin(2 * lat_rad) +
-        WGS84_a3 * sin(4 * lat_rad) -
-        WGS84_a4 * sin(6 * lat_rad)
-    );
-
-    double lon_origin = (getUTMZoneNumber(latitude, longitude) - 1) * 6 - 180 + 3;
-    lon_origin = deg2rad(lon_origin);
-
-    double T = tan(lat_rad) * tan(lat_rad);
-    double C = WGS84_ep2 * cos(lat_rad) * cos(lat_rad);
-    double A = (lon_rad - lon_origin) * cos(lat_rad);
-    double A2 = A * A;
-    double A3 = A2 * A;
-    double A4 = A2 * A2;
-    double A5 = A4 * A;
-    double A6 = A4 * A2;
-
-    easting = 500000 + UTM_k0 * N * (
-        A + (1 - T + C) * A3 / 6 +
-        (5 - 18 * T + T * T + 72 * C - 58 * WGS84_ep2) * A5 / 120
-    );
-
-    northing = M + N * tan(lat_rad) * (
-        A2 / 2 + (5 - T + 9 * C + 4 * C *C) * A4 / 24 +
-        (61 - 58 * T + T * T + 600 * C - 330 * WGS84_ep2) * A6 / 720
-    );
-    northing *= UTM_k0;
-}
-
-
-void NE2latlon(double northing, double easting,
-               double &latitude, double &longitude,
-               int zone)
-{
-    double x = easting - 500000;
-    double y = northing;
-
-    const static double e1 = (1 - sqrt(1 - WGS84_e2)) / (1 + sqrt(1 - WGS84_e2));
-    const static double e2 = e1 * e1;
-    const static double e3 = e2 * e1;
-    const static double e4 = e3 * e1;
-
-    double lon_origin = (zone - 1) * 6 - 180 + 3;
-    double M = y / UTM_k0;
-    double mu = M / (
-        WGS84_a * (
-            1 - WGS84_e2 / 4 - (3 * WGS84_e4) / 64 - (5 * WGS84_e6) / 256
-        )
-    );
-    double phi1 = mu + (3 * e1 / 2 - 27 * e3 / 32) * sin(2 * mu) +
-        (21 * e2 / 16 - 55 * e4 / 32) * sin(4 * mu) +
-        (151 * e3 / 96) * sin(6 * mu);
-
-    double z = sqrt(1 - WGS84_e2 * sin(phi1) * sin(phi1));
-    double N1 = WGS84_a / z;
-    double T1 = tan(phi1) * tan(phi1);
-    double C1 = WGS84_ep2 * cos(phi1) * cos(phi1);
-    double R1 = WGS84_a * (1 - WGS84_e2) / (z * z * z);
-    double D = x / (N1 * UTM_k0);
-    double D2 = D * D;
-    double D3 = D2 * D;
-    double D4 = D2 * D2;
-    double D5 = D4 * D;
-    double D6 = D4 * D2;
-
-    latitude = phi1 - (N1 * tan(phi1) / R1) * (
-        D2 / 2 -
-        (5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * WGS84_ep2) * D4 / 24 +
-        (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * WGS84_ep2 - 3 * C1 * C1) * D6 / 720
-    );
-    latitude = rad2deg(latitude);
-
-    longitude = (
-        D - ( 1 + 2 * T1 * C1) * D3 / 6 +
-        (5 - 2 *C1 + 28 * T1 - 3 * C1 * C1 + 8 * WGS84_ep2 + 24 *T1 * T1) * D5 / 120
-    ) / cos(phi1);
-
-    longitude = rad2deg(longitude) + lon_origin;
-}
-
-}
-}

src/decition/decition_brain/decision/gnss_proj/gnss_proj.h

@@ -1,48 +0,0 @@
-/*
- * --------------------------------------------
- * 简易版本的 (纬度, 经度) 和 UTM (北, 东) 坐标系互转
- * --------------------------------------------
- *
- * 在线验证地址 https://www.latlong.net/lat-long-utm.html
- *
- * 头文件提供 5 个函数的接口:
- *
- * 1. latlon2NE 用于将 (纬度，经度) 转换为 UTM 坐标系下 (北，东) 坐标。
- * 2. NE2latlon 用于将 UTM 下 (北，东) 坐标转换为 (纬度，经度)。
- * 3. getUTMZoneNumber 用于获得给定纬经度点所在的 UTM 时区编号，返回值为整数，
- *    如天津地区返回 50。
- * 4. getUTMLetterDesignator 用于获得给定纬经度点所在的 UTM 时区编码，
- *    返回值为字符，如天津地区返回 'S'。
- * 5. getUTMZone 是上面两个函数的组合，返回 UTM 坐标时区，返回值是字符串，
- *    如天津地区返回 '50S'。
- *
- * 转换公式参考:
- *
- * 1. 大地测量学基础. 孔祥元. 武汉大学出版社.
- * 2. Conformal Map Projections in Geodesy. Krakiwsky. E.J.
-*/
-
-#ifndef GNSS_COORDINATE_CONVERSION_H
-#define GNSS_COORDINATE_CONVERSION_H
-
-#pragma once
-
-namespace iv {
-namespace localization {
-
-int getUTMZoneNumber(double latitude, double longitude);
-
-char getUTMLetterDesignator(double latitude);
-
-void getUTMZone(double latitude, double longitude, char* UTMZone);
-
-void latlon2NE(double latitude, double longitude,
-               double& northing, double& easting);
-
-void NE2latlon(double northing, double easting,
-               double& latitude, double& longitude,
-               int zone = 50);
-
-}
-}
-#endif // GNSS_COORDINATE_CONVERSION_H

src/decition/decition_brain/decision/gnss_proj/gnss_proj.pri

@@ -1,5 +0,0 @@
-HEADERS += \
-    $$PWD/gnss_proj.h
-
-SOURCES += \
-    $$PWD/gnss_proj.cpp

src/decition/decition_brain/decision/gps_distance.cpp

@@ -1,45 +0,0 @@
-#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/decision/gps_distance.h

@@ -1,26 +0,0 @@
-#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/decision/obs_mobieye.cpp

@@ -1,388 +0,0 @@
-#include "obs_mobieye.h"
-
-#include <math.h>
-
-#include "ivlog.h"
-
-extern iv::Ivlog * givlog;
-
-obs_mobieye * gom;
-
-static void ListenMobieye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    (void)&index;
-    (void)dt;
-    (void)strmemname;
-    iv::mobileye::mobileye xmobieye;
-    if(xmobieye.ParseFromArray(strdata,nSize))
-    {
-        gom->UpdateMobieyeObs(xmobieye);
-    }
-}
-
-obs_mobieye::obs_mobieye()
-{
-    gom = this;
-    mpa = iv::modulecomm::RegisterRecv("mobileye",ListenMobieye);
-    mfangajust = 0.05;
-}
-
-obs_mobieye & obs_mobieye::GetInst()
-{
-    static obs_mobieye x;
-    return x;
-}
-
-void obs_mobieye::UpdateMobieyeObs(iv::mobileye::mobileye &pmobieye)
-{
-    mMutex.lock();
-    mmobieye.CopyFrom(pmobieye);
-    mnUpdateTime = QDateTime::currentMSecsSinceEpoch();
-    mMutex.unlock();
-}
-
-void obs_mobieye::InterpolationTrace(std::vector<iv::Point2D> *pgpsTrace, std::vector<iv::Point2D> &xgpstrace)
-{
-    xgpstrace = *pgpsTrace;
-    unsigned int k;
-    for(k=0;k<(xgpstrace.size()-1);k++)
-    {
-        double fdis;
-        fdis = sqrt(pow(xgpstrace[k].x - xgpstrace[k+1].x,2)
-                    +pow(xgpstrace[k].y - xgpstrace[k+1].y,2));
-        if(fdis > 0.16)
-        {
-            qDebug("fdis %f is big, insert a point2d ",fdis);
-            iv::Point2D xpoint;
-            xpoint.x = (xgpstrace[k].x + xgpstrace[k+1].x)/2.0;
-            xpoint.y = (xgpstrace[k].y + xgpstrace[k+1].y)/2.0;
-
-            xgpstrace.insert((xgpstrace.begin() +k+1),xpoint);
-            if(k>0)k = k-1;
-        }
-    }
-}
-
-int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobileye::mobileye &xmobieye,
-                                 const double fveh_width,std::vector<iv::mobieyeobstotrace>  & xvectoroptobs)
-{
-    unsigned int i;
-    unsigned int nobjsize = xmobieye.xobj_size();
-
-    givlog->debug("MOBS","obs size is %d",nobjsize);
-
-    for(i=0;i<nobjsize;i++)
-    {
-        iv::mobileye::obs * pobs = xmobieye.mutable_xobj(i);
-
-        double fobs_x = pobs->pos_y()*(-1);
-        double fobs_y = pobs->pos_x();
-        ajustobspos(fobs_x,fobs_y,mfangajust);
-
-        givlog->debug("MOBS","obs pos X:%f y:%f",fobs_x,fobs_y);
-
-        unsigned int j;
-        unsigned int ntracecount = xgpstrace.size();
-        double fdismin = 1000.0;
-        int index = -1;
-        for(j=0;j<ntracecount;j++)
-        {
-            if(fabs(fobs_x - xgpstrace[j].x) > 10.0)
-            {
-                continue;
-            }
-            if(fabs(fobs_y - xgpstrace[j].y) > 10.0)
-            {
-                continue;
-            }
-            double fdis = sqrt(pow(xgpstrace[j].x - fobs_x,2)
-                               +pow(xgpstrace[j].y - fobs_y,2));
-            if(fdis<fdismin)
-            {
-                fdismin = fdis;
-                index = j;
-            }
-
-        }
-
-        if(fdismin < (fveh_width + pobs->obswidth()/2.0))
-        {
-            iv::mobieyeobstotrace xmt;
-            xmt.obsindex = i;
-            xmt.traceindex = index;
-            xmt.fdis = fdismin;
-            xvectoroptobs.push_back(xmt);
-        }
-    }
-
-}
-
-
-void obs_mobieye::ajustobspos(double &fobs_x, double fobs_y, double fang)
-{
-    double xtem = fobs_x;
-    double ytem = fobs_y;
-    fobs_x = xtem *cos(fang) - ytem*sin(fang);
-    fobs_y = xtem *sin(fang) + ytem*cos(fang);
-
-}
-
-
-bool obs_mobieye::PointInObs(iv::mobileye::obs *pobs, iv::Point2D xpoint)
-{
-    double x,y;
-    x = xpoint.x;
-    y = xpoint.y;
-    double rel_x,rel_y;
-
-    double fobs_x = pobs->pos_y()*(-1);
-    double fobs_y = pobs->pos_x();
-    ajustobspos(fobs_x,fobs_y,mfangajust);
-
-    rel_x = x - fobs_x;
-    rel_y = y - fobs_y;
-    double beta = pobs->obsang() *M_PI/180.0;
-    double xrot,yrot;
-    xrot = rel_x *cos(beta) - rel_y*sin(beta);
-    yrot = rel_x *sin(beta) + rel_y*cos(beta);
-
-    if(fabs(xrot)<=(0.2 + pobs->obswidth()/2.0))
-    {
-
-        return true;
-    }
-
-//    if(fabs(yrot)<=(pobs->obslen()/2.0))
-//    {
-//        return true;
-//    }
-
-    return false;
-}
-
-void obs_mobieye::GetVehiclePoint(std::vector<iv::Point2D> &xgpstrace, int index,
-                                  std::vector<iv::Point2D> &xvectorPoint, const double fveh_width)
-{
-    if(index<0)return;
-    if(index>=xgpstrace.size())
-    {
-        return;
-    }
-
-    double hdg = 0;
-    if(index<(xgpstrace.size()-1))
-    {
-        hdg = CalcHdg(QPointF(xgpstrace[index].x,xgpstrace[index].y),
-                      QPointF(xgpstrace[index+1].x,xgpstrace[index+1].y));
-    }
-    else
-    {
-        hdg = CalcHdg(QPointF(xgpstrace[index-1].x,xgpstrace[index-1].y),
-                      QPointF(xgpstrace[index].x,xgpstrace[index].y));
-    }
-
-    hdg = hdg - M_PI/2.0;
-
-
-    const double fdiv = 0.1;
-    int ncount = fveh_width/fdiv;
-    int i;
-    double x0,y0;
-    x0 = xgpstrace[index].x;
-    y0 = xgpstrace[index].y;
-    for(i=0;i<ncount;i++)
-    {
-        double xtem,ytem;
-        double xp,yp;
-        xtem = (i-ncount/2) * fdiv;
-        ytem = 0;
-        xp = xtem*cos(hdg) - ytem*sin(hdg) + x0;
-        yp = xtem*sin(hdg) + ytem*cos(hdg) + y0;
-        iv::Point2D pointx;
-        pointx.x = xp;
-        pointx.y = yp;
-        xvectorPoint.push_back(pointx);
-    }
-}
-
-double obs_mobieye::CalcHdg(QPointF p0, QPointF p1)
-{
-    double x0,y0,x1,y1;
-    x0 = p0.x();
-    y0 = p0.y();
-    x1 = p1.x();
-    y1 = p1.y();
-    if(x0 == x1)
-    {
-        if(y0 < y1)
-        {
-            return M_PI/2.0;
-        }
-        else
-            return M_PI*3.0/2.0;
-    }
-
-    double ratio = (y1-y0)/(x1-x0);
-
-    double hdg = atan(ratio);
-
-    if(ratio > 0)
-    {
-        if(y1 > y0)
-        {
-
-        }
-        else
-        {
-            hdg = hdg + M_PI;
-        }
-    }
-    else
-    {
-        if(y1 > y0)
-        {
-            hdg = hdg + M_PI;
-        }
-        else
-        {
-            hdg = hdg + 2.0*M_PI;
-        }
-    }
-
-    return hdg;
-}
-
-double obs_mobieye::GetTraceDis(std::vector<iv::Point2D> &xgpstrace, int index)
-{
-    double fdis = 0;
-    int i;
-    for(i=1;i<=index;i++)
-    {
-        fdis = fdis + sqrt(pow(xgpstrace[i-1].x-xgpstrace[i].x,2)
-                    +pow(xgpstrace[i-1].y - xgpstrace[i].y,2));
-    }
-    return fdis;
-}
-
-int obs_mobieye::GetObs(std::vector<iv::Point2D> &xgpstrace, iv::mobileye::mobileye &xmobieye,
-                        const double fveh_width, std::vector<iv::mobieyeobstotrace> &xvectoroptobs,
-                        double &xobsdis, double &xobsspeed, double &xttc, double &xobsid)
-{
-//    std::vector<QPointF> xPointTrace;
-//    std::vector<QPointF> xPointObs;
-    unsigned int i;
-
-
-    std::vector<iv::candidateobs> xvectorcand;
-    for(i=0;i<xvectoroptobs.size();i++)
-    {
-        std::vector<iv::Point2D> xvectorPoint;
-        xvectorPoint.clear();
-        GetVehiclePoint(xgpstrace,xvectoroptobs[i].traceindex,xvectorPoint,fveh_width);
-        bool bInObs = false;
-        unsigned int j;
-        iv::mobileye::obs * pobs = xmobieye.mutable_xobj(xvectoroptobs[i].obsindex);
-//        if(xvectoroptobs[i].fdis < fveh_width/2.0)
-//        {
-//            bInObs = true;
-//        }
-        for(j=0;j<xvectorPoint.size();j++)
-        {
-            if(PointInObs(pobs,xvectorPoint[j]))
-            {
-                bInObs = true;
-                break;
-            }
-        }
-        if(bInObs)
-        {
-//             givlog->debug("MOBS","have obs");
-            iv::candidateobs x;
-            x.xobsdis = GetTraceDis(xgpstrace,xvectoroptobs[i].traceindex);
-            x.xttc = 15;
-            if((x.xobsdis>0)&&(pobs->obs_rel_vel_x()!= 0 ))x.xttc = x.xobsdis/pobs->obs_rel_vel_x();
-            x.xobsspeed = pobs->obs_rel_vel_x();
-            x.xobsid = pobs->id();
-            xvectorcand.push_back(x);
-        }
-    }
-
-    for(i=0;i<xvectorcand.size();i++)
-    {
-        if((xvectorcand[i].xttc>0)&&(xvectorcand[i].xttc<= 15))
-        {
-            if((xttc>15)||(xttc<0))
-            {
-                xttc = xvectorcand[i].xttc;
-                xobsspeed = xvectorcand[i].xobsspeed;
-                xobsdis = xvectorcand[i].xobsdis;
-                xobsid = xvectorcand[i].xobsid;
-            }
-            else
-            {
-                if(xttc>xvectorcand[i].xttc)
-                {
-                    xttc = xvectorcand[i].xttc;
-                    xobsspeed = xvectorcand[i].xobsspeed;
-                    xobsdis = xvectorcand[i].xobsdis;
-                    xobsid = xvectorcand[i].xobsid;
-                }
-            }
-        }
-        else
-        {
-            if((xttc>15)||(xttc<0))
-            {
-                xttc = xvectorcand[i].xttc;
-                xobsspeed = xvectorcand[i].xobsspeed;
-                xobsdis = xvectorcand[i].xobsdis;
-                xobsid = xvectorcand[i].xobsid;
-            }
-        }
-
-    }
-
-
-
-    return 0;
-}
-
-int obs_mobieye::GetObsFromMobieye(std::vector<iv::Point2D> *pgpsTrace,
-                                   double &xobsdis, double &xobsspeed, double &xttc, double &xobsid,const double fveh_width)
-{
-    xobsdis = 500;
-    xobsspeed = 0;
-    xttc = -1;
-    xobsid = -1;
-    if(pgpsTrace->size()<1)return -1;
-    iv::mobileye::mobileye xmobieye;
-    if((QDateTime::currentMSecsSinceEpoch() - mnUpdateTime) > 3000)
-    {
-        qDebug("more than 3 secons no mobieye data.");
-        return -2;
-    }
-
-    mMutex.lock();
-    xmobieye.CopyFrom(mmobieye);
-    mMutex.unlock();
-
-
-    std::vector<iv::Point2D> xgpstrace;
-
-    InterpolationTrace(pgpsTrace,xgpstrace);
-
-
-    std::vector<iv::mobieyeobstotrace> xvectoroptobs;
-
-    GetCandidataObs(xgpstrace,xmobieye,fveh_width,xvectoroptobs);
-
-    if(xvectoroptobs.size() < 1)
-    {
-        return 0;
-    }
-
-    GetObs(xgpstrace,xmobieye,fveh_width,xvectoroptobs,xobsdis,xobsspeed,xttc,xobsid);
-
-    return 0;
-
-}

src/decition/decition_brain/decision/obs_mobieye.h

@@ -1,84 +0,0 @@
-#ifndef OBS_MOBIEYE_H
-#define OBS_MOBIEYE_H
-
-#include <QMutex>
-#include <QPointF>
-
-#include "mobileye.pb.h"
-
-#include "modulecomm.h"
-
-#include <common/gps_type.h>
-
-
-namespace  iv {
-struct  mobieyeobstotrace
-{
-public:
-    int traceindex;
-    int obsindex;
-    double fdis;
-};
-
-}
-
-namespace iv {
-struct candidateobs
-{
-    double xobsdis;
-    double xobsspeed;
-    double xttc;
-    double xobsid;
-};
-
-}
-
-class obs_mobieye
-{
-public:
-    obs_mobieye();
-
-private:
-    void * mpa;
-
-    iv::mobileye::mobileye mmobieye;
-    qint64 mnUpdateTime;
-    QMutex mMutex;
-
-public:
-    void UpdateMobieyeObs(iv::mobileye::mobileye & pmobieye);
-    static obs_mobieye & GetInst();
-public:
-    int GetObsFromMobieye(std::vector<iv::Point2D> * pgpsTrace,double & xobsdis, double & xobsspeed, double & xttc,
-                          double & xobsid,const double fveh_width);
-
-
-public:
-    static double CalcHdg(QPointF p0, QPointF p1);
-
-private:
-    void InterpolationTrace(std::vector<iv::Point2D> * pgpsTrace,std::vector<iv::Point2D> & xgpstrace);
-    int GetCandidataObs(std::vector<iv::Point2D> & xgpstrace,iv::mobileye::mobileye & xmobieye,
-                        const double fveh_width,std::vector<iv::mobieyeobstotrace>  & xvectoroptobs);
-
-    int GetObs(std::vector<iv::Point2D> & xgpstrace,iv::mobileye::mobileye & xmobieye,
-               const double fveh_width,std::vector<iv::mobieyeobstotrace>  & xvectoroptobs,
-               double & xobsdis, double & xobsspeed, double & xttc,
-                                         double & xobsid);
-
-    bool PointInObs(iv::mobileye::obs * pobs, iv::Point2D xpoint);
-
-    void GetVehiclePoint(std::vector<iv::Point2D> & xgpstrace,int index,std::vector<iv::Point2D> & xvectorPoint,
-                         const double fveh_width);
-
-    double GetTraceDis(std::vector<iv::Point2D> & xgpstrace,int index);
-
-    double mfangajust = 0.0;
-
-    void ajustobspos(double & fobs_x,double fobs_y,double fang);
-
-};
-
-#define MobieyeInst obs_mobieye::GetInst()
-
-#endif // OBS_MOBIEYE_H

src/decition/decition_brain/decision/obs_predict.cpp

@@ -1,118 +0,0 @@
-#include <decision/obs_predict.h>
-#include <decision/decition_type.h>
-#include <common/gps_type.h>
-#include <math.h>
-#include <iostream>
-#include <fstream>
-#include <decision/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>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 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/decision/obs_predict.h

@@ -1,22 +0,0 @@
-#ifndef OBS_PREDICT_H
-#define OBS_PREDICT_H
-
-#include <common/gps_type.h>
-#include <common/obstacle_type.h>
-#include <decision/decition_type.h>
-#include <vector>
-#include <decision/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/decision/tinyspline/tinyspline.c

@@ -1,1400 +0,0 @@
-#include <tinyspline_ros/tinyspline.h>
-
-#include <stdlib.h> /* malloc, free */
-#include <math.h> /* fabs, sqrt */
-#include <string.h> /* memcpy, memmove, strcmp */
-#include <setjmp.h> /* setjmp, longjmp */
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Error handling                                                           *
-*                                                                             *
-******************************************************************************/
-#define TRY(x, y) y = (tsError) setjmp(x); if (y == 0) {
-#define CATCH } else {
-#define ETRY }
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Data Types                                                               *
-*                                                                             *
-******************************************************************************/
-/**
- * Stores the private data of a ::tsBSpline.
- */
-struct tsBSplineImpl
-{
-	size_t deg; /**< Degree of B-Spline basis function. */
-	size_t dim; /**< Dimension of control points (2D => x, y) */
-	size_t n_ctrlp; /**< Number of control points. */
-	size_t n_knots; /**< Number of knots (n_ctrlp + deg + 1). */
-};
-
-/**
- * Stores the private data of a ::tsDeBoorNet.
- */
-struct tsDeBoorNetImpl
-{
-	tsReal u; /**< The evaluated knot value. */
-	size_t k; /**< The index [u_k, u_k+1) */
-	size_t s; /**< Multiplicity of u_k. */
-	size_t h; /**< Number of insertions required to obtain result. */
-	size_t dim; /**< Dimension of points. (2D => x, y) */
-	size_t n_points; /** Number of points in 'points'. */
-};
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Forward Declarations & Internal Utility Functions                        *
-*                                                                             *
-******************************************************************************/
-void ts_internal_bspline_fill_knots(const tsBSpline *spline,
-	tsBSplineType type, tsReal min, tsReal max, tsBSpline *_result_,
-	jmp_buf buf);
-
-size_t ts_internal_bspline_sof_state(const tsBSpline *spline)
-{
-	return sizeof(struct tsBSplineImpl) +
-	       ts_bspline_sof_control_points(spline) +
-	       ts_bspline_sof_knots(spline);
-}
-
-tsReal * ts_internal_bspline_access_ctrlp(const tsBSpline *spline)
-{
-	return (tsReal *) (& spline->pImpl[1]);
-}
-
-tsReal * ts_internal_bspline_access_knots(const tsBSpline *spline)
-{
-	return ts_internal_bspline_access_ctrlp(spline)
-		+ ts_bspline_len_control_points(spline);
-}
-
-size_t ts_internal_deboornet_sof_state(const tsDeBoorNet *net)
-{
-	return sizeof(struct tsDeBoorNetImpl) +
-	       ts_deboornet_sof_points(net) +
-	       ts_deboornet_sof_result(net);
-}
-
-tsReal * ts_internal_deboornet_access_points(const tsDeBoorNet *net)
-{
-	return (tsReal *) (& net->pImpl[1]);
-}
-
-tsReal * ts_internal_deboornet_access_result(const tsDeBoorNet *net)
-{
-	if (ts_deboornet_num_result(net) == 2) {
-		return ts_internal_deboornet_access_points(net);
-	} else {
-		return ts_internal_deboornet_access_points(net) +
-			/* Last point in `points`. */
-		       (ts_deboornet_len_points(net) -
-			ts_deboornet_dimension(net));
-	}
-}
-
-void ts_internal_bspline_find_u(const tsBSpline *spline, tsReal u, size_t *k,
-	size_t *s, jmp_buf buf)
-{
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t order = ts_bspline_order(spline);
-	const size_t num_knots = ts_bspline_num_knots(spline);
-	const tsReal *knots = ts_internal_bspline_access_knots(spline);
-
-	*k = *s = 0;
-	for (; *k < num_knots; (*k)++) {
-		const tsReal uk = knots[*k];
-		if (ts_fequals(u, uk)) {
-			(*s)++;
-		} else if (u < uk) {
-			break;
-		}
-	}
-
-	/* keep in mind that currently k is k+1 */
-	if (*s > order)
-		longjmp(buf, TS_MULTIPLICITY);
-	if (*k <= deg)                /* u < u_min */
-		longjmp(buf, TS_U_UNDEFINED);
-	if (*k == num_knots && *s == 0) /* u > u_last */
-		longjmp(buf, TS_U_UNDEFINED);
-	if (*k > num_knots-deg + *s-1)  /* u > u_max */
-		longjmp(buf, TS_U_UNDEFINED);
-
-	(*k)--; /* k+1 - 1 will never underflow */
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Field Access Functions                                                   *
-*                                                                             *
-******************************************************************************/
-size_t ts_bspline_degree(const tsBSpline *spline)
-{
-	return spline->pImpl->deg;
-}
-
-tsError ts_bspline_set_degree(tsBSpline *spline, size_t deg)
-{
-	if (deg >= ts_bspline_num_control_points(spline))
-		return TS_DEG_GE_NCTRLP;
-	spline->pImpl->deg = deg;
-	return TS_SUCCESS;
-}
-
-size_t ts_bspline_order(const tsBSpline *spline)
-{
-	return ts_bspline_degree(spline) + 1;
-}
-
-tsError ts_bspline_set_order(tsBSpline *spline, size_t order)
-{
-	return ts_bspline_set_degree(spline, order - 1);
-}
-
-size_t ts_bspline_dimension(const tsBSpline *spline)
-{
-	return spline->pImpl->dim;
-}
-
-tsError ts_bspline_set_dimension(tsBSpline *spline, size_t dim)
-{
-	if (dim == 0)
-		return TS_DIM_ZERO;
-	if (ts_bspline_len_control_points(spline) % dim != 0)
-		return TS_LCTRLP_DIM_MISMATCH;
-	spline->pImpl->dim = dim;
-	return TS_SUCCESS;
-}
-
-size_t ts_bspline_len_control_points(const tsBSpline *spline)
-{
-	return ts_bspline_num_control_points(spline) *
-	       ts_bspline_dimension(spline);
-}
-
-size_t ts_bspline_num_control_points(const tsBSpline *spline)
-{
-	return spline->pImpl->n_ctrlp;
-}
-
-size_t ts_bspline_sof_control_points(const tsBSpline *spline)
-{
-	return ts_bspline_len_control_points(spline) * sizeof(tsReal);
-}
-
-tsError ts_bspline_control_points(const tsBSpline *spline, tsReal **ctrlp)
-{
-	const size_t size = ts_bspline_sof_control_points(spline);
-	*ctrlp = malloc(size);
-	if (!*ctrlp) {
-		return TS_MALLOC;
-	} else {
-		memcpy(*ctrlp,
-		       ts_internal_bspline_access_ctrlp(spline),
-		       size);
-		return TS_SUCCESS;
-	}
-}
-
-tsError ts_bspline_set_control_points(tsBSpline *spline, const tsReal *ctrlp)
-{
-	const size_t size = ts_bspline_sof_control_points(spline);
-	memcpy(ts_internal_bspline_access_ctrlp(spline), ctrlp, size);
-	return TS_SUCCESS;
-}
-
-size_t ts_bspline_num_knots(const tsBSpline *spline)
-{
-	return spline->pImpl->n_knots;
-}
-
-size_t ts_bspline_sof_knots(const tsBSpline *spline)
-{
-	return ts_bspline_num_knots(spline) * sizeof(tsReal);
-}
-
-tsError ts_bspline_knots(const tsBSpline *spline, tsReal **knots)
-{
-	const size_t size = ts_bspline_sof_knots(spline);
-	*knots = malloc(size);
-	if (!*knots) {
-		return TS_MALLOC;
-	} else {
-		memcpy(*knots,
-		       ts_internal_bspline_access_knots(spline),
-		       size);
-		return TS_SUCCESS;
-	}
-}
-
-tsError ts_bspline_set_knots(tsBSpline *spline, const tsReal *knots)
-{
-	size_t order, size, idx, mult;
-	tsReal lst_knot, knot;
-	order = ts_bspline_order(spline);
-	lst_knot = ts_internal_bspline_access_knots(spline)[0];
-	mult = 1;
-	for (idx = 1; idx < ts_bspline_num_knots(spline); idx++)
-	{
-		knot = knots[idx];
-		if (ts_fequals(lst_knot, knot))
-			mult++;
-		else if (lst_knot > knot)
-			return TS_KNOTS_DECR;
-		else
-			mult = 0;
-		if (mult > order)
-			return TS_MULTIPLICITY;
-		lst_knot = knot;
-	}
-	size = ts_bspline_sof_knots(spline);
-	memcpy(ts_internal_bspline_access_knots(spline), knots, size);
-	return TS_SUCCESS;
-}
-
-/* ------------------------------------------------------------------------- */
-
-tsReal ts_deboornet_knot(const tsDeBoorNet *net)
-{
-	return net->pImpl->u;
-}
-
-size_t ts_deboornet_index(const tsDeBoorNet *net)
-{
-	return net->pImpl->k;
-}
-
-size_t ts_deboornet_multiplicity(const tsDeBoorNet *net)
-{
-	return net->pImpl->s;
-}
-
-size_t ts_deboornet_num_insertions(const tsDeBoorNet *net)
-{
-	return net->pImpl->h;
-}
-
-size_t ts_deboornet_dimension(const tsDeBoorNet *net)
-{
-	return net->pImpl->dim;
-}
-
-size_t ts_deboornet_len_points(const tsDeBoorNet *net)
-{
-	return ts_deboornet_num_points(net) * ts_deboornet_dimension(net);
-}
-
-size_t ts_deboornet_num_points(const tsDeBoorNet *net)
-{
-	return net->pImpl->n_points;
-}
-
-size_t ts_deboornet_sof_points(const tsDeBoorNet *net)
-{
-	return ts_deboornet_len_points(net) * sizeof(tsReal);
-}
-
-tsError ts_deboornet_points(const tsDeBoorNet *net, tsReal **points)
-{
-	const size_t size = ts_deboornet_sof_points(net);
-	*points = malloc(size);
-	if (!*points) {
-		return TS_MALLOC;
-	} else {
-		memcpy(*points,
-		       ts_internal_deboornet_access_points(net),
-		       size);
-		return TS_SUCCESS;
-	}
-}
-
-size_t ts_deboornet_len_result(const tsDeBoorNet *net)
-{
-	return ts_deboornet_num_result(net) * ts_deboornet_dimension(net);
-}
-
-size_t ts_deboornet_num_result(const tsDeBoorNet *net)
-{
-	return ts_deboornet_num_points(net) == 2 ? 2 : 1;
-}
-
-size_t ts_deboornet_sof_result(const tsDeBoorNet *net)
-{
-	return ts_deboornet_len_result(net) * sizeof(tsReal);
-}
-
-tsError ts_deboornet_result(const tsDeBoorNet *net, tsReal **result)
-{
-	const size_t size = ts_deboornet_sof_result(net);
-	*result = malloc(size);
-	if (!*result) {
-		return TS_MALLOC;
-	} else {
-		memcpy(*result,
-		       ts_internal_deboornet_access_result(net),
-		       size);
-		return TS_SUCCESS;
-	}
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Constructors, Destructors, Copy, and Move Functions                      *
-*                                                                             *
-******************************************************************************/
-void ts_bspline_default(tsBSpline *_spline_)
-{
-	_spline_->pImpl = NULL;
-}
-
-void ts_internal_bspline_new(size_t n_ctrlp, size_t dim, size_t deg,
-	tsBSplineType type, tsBSpline *_spline_, jmp_buf buf)
-{
-	const size_t order = deg + 1;
-	const size_t num_knots = n_ctrlp + order;
-	const size_t len_ctrlp = n_ctrlp * dim;
-
-	const size_t sof_real = sizeof(tsReal);
-	const size_t sof_impl = sizeof(struct tsBSplineImpl);
-	const size_t sof_ctrlp_vec = len_ctrlp * sof_real;
-	const size_t sof_knots_vec = num_knots * sof_real;
-	const size_t sof_spline = sof_impl + sof_ctrlp_vec + sof_knots_vec;
-
-	tsError e;
-	jmp_buf b;
-
-	if (dim < 1)
-		longjmp(buf, TS_DIM_ZERO);
-	if (deg >= n_ctrlp)
-		longjmp(buf, TS_DEG_GE_NCTRLP);
-
-	_spline_->pImpl = (struct tsBSplineImpl *) malloc(sof_spline);
-	if (!_spline_->pImpl)
-		longjmp(buf, TS_MALLOC);
-
-	_spline_->pImpl->deg = deg;
-	_spline_->pImpl->dim = dim;
-	_spline_->pImpl->n_ctrlp = n_ctrlp;
-	_spline_->pImpl->n_knots = num_knots;
-
-	TRY(b, e)
-		ts_internal_bspline_fill_knots(
-			_spline_, type, 0.f, 1.f, _spline_, b);
-	CATCH
-		ts_bspline_free(_spline_);
-		longjmp(buf, e);
-	ETRY
-}
-
-tsError ts_bspline_new(size_t n_ctrlp, size_t dim, size_t deg,
-	tsBSplineType type, tsBSpline *_spline_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_new(
-			n_ctrlp, dim, deg, type, _spline_, buf);
-	CATCH
-		ts_bspline_default(_spline_);
-	ETRY
-	return err;
-}
-
-void ts_internal_bspline_copy(const tsBSpline *original, tsBSpline *_copy_,
-	jmp_buf buf)
-{
-	size_t size;
-	if (original == _copy_)
-		return;
-	size = ts_internal_bspline_sof_state(original);
-	_copy_->pImpl = malloc(size);
-	if (!_copy_->pImpl)
-		longjmp(buf, TS_MALLOC);
-	memcpy(_copy_->pImpl, original->pImpl, size);
-}
-
-tsError ts_bspline_copy(const tsBSpline *original, tsBSpline *_copy_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_copy(original, _copy_, buf);
-	CATCH
-		if (original != _copy_)
-			ts_bspline_default(_copy_);
-	ETRY
-	return err;
-}
-
-void ts_bspline_move(tsBSpline *from, tsBSpline *_to_)
-{
-	if (from == _to_)
-		return;
-	_to_->pImpl = from->pImpl;
-	ts_bspline_default(from);
-}
-
-void ts_bspline_free(tsBSpline *_spline_)
-{
-	if (_spline_->pImpl)
-		free(_spline_->pImpl);
-	ts_bspline_default(_spline_);
-}
-
-/* ------------------------------------------------------------------------- */
-
-void ts_deboornet_default(tsDeBoorNet *_deBoorNet_)
-{
-	_deBoorNet_->pImpl = NULL;
-}
-
-void ts_internal_deboornet_new(const tsBSpline *spline,
-	tsDeBoorNet *_deBoorNet_, jmp_buf buf)
-{
-	const size_t dim = ts_bspline_dimension(spline);
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t num_points = (size_t)(deg * (deg+1) * 0.5f);
-
-	const size_t sof_real = sizeof(tsReal);
-	const size_t sof_impl = sizeof(struct tsDeBoorNetImpl);
-	const size_t sof_points_vec = num_points * dim * sof_real;
-	const size_t sof_net = sof_impl * sof_points_vec;
-
-	_deBoorNet_->pImpl = (struct tsDeBoorNetImpl *) malloc(sof_net);
-	if (!_deBoorNet_->pImpl)
-		longjmp(buf, TS_MALLOC);
-
-	_deBoorNet_->pImpl->u = 0.f;
-	_deBoorNet_->pImpl->k = 0;
-	_deBoorNet_->pImpl->s = 0;
-	_deBoorNet_->pImpl->h = deg;
-	_deBoorNet_->pImpl->dim = dim;
-	_deBoorNet_->pImpl->n_points = num_points;
-}
-
-void ts_deboornet_free(tsDeBoorNet *_deBoorNet_)
-{
-	if (_deBoorNet_->pImpl)
-		free(_deBoorNet_->pImpl);
-	ts_deboornet_default(_deBoorNet_);
-}
-
-void ts_internal_deboornet_copy(const tsDeBoorNet *original,
-	tsDeBoorNet *_copy, jmp_buf buf)
-{
-	size_t size;
-	if (original == _copy)
-		return;
-	size = ts_internal_deboornet_sof_state(original);
-	_copy->pImpl = malloc(size);
-	if (!_copy->pImpl)
-		longjmp(buf, TS_MALLOC);
-	memcpy(_copy->pImpl, original->pImpl, size);
-}
-
-tsError ts_deboornet_copy(const tsDeBoorNet *original, tsDeBoorNet *_copy_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_deboornet_copy(original, _copy_, buf);
-	CATCH
-		if (original != _copy_)
-			ts_deboornet_default(_copy_);
-	ETRY
-	return err;
-}
-
-void ts_deboornet_move(tsDeBoorNet *from, tsDeBoorNet *_to_)
-{
-	if (from == _to_)
-		return;
-	_to_->pImpl = from->pImpl;
-	ts_deboornet_default(from);
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Interpolation and Approximation Functions                                *
-*                                                                             *
-******************************************************************************/
-void ts_internal_bspline_thomas_algorithm(const tsReal *points, size_t n,
-	size_t dim, tsReal *_result_, jmp_buf buf)
-{
-	const size_t sof_real = sizeof(tsReal);
-	const size_t sof_ctrlp = dim * sof_real;
-	tsReal* m;      /* Array of weights. */
-	size_t len_m;   /* Length of m. */
-	size_t lst;     /* Index of the last control point in \p points. */
-	size_t i, d;    /* Used in for loops. */
-	size_t j, k, l; /* Used as temporary indices. */
-
-	/* input validation */
-	if (dim == 0)
-		longjmp(buf, TS_DIM_ZERO);
-	if (n == 0)
-		longjmp(buf, TS_DEG_GE_NCTRLP);
-
-	if (n <= 2) {
-		memcpy(_result_, points, n * sof_ctrlp);
-		return;
-	}
-
-	/* In the following n >= 3 applies... */
-	len_m = n-2; /* ... len_m >= 1 */
-	lst = (n-1)*dim; /* ... lst >= 2*dim */
-
-	/* m_0 = 1/4, m_{k+1} = 1/(4-m_k), for k = 0,...,n-2 */
-	m = (tsReal*) malloc(len_m * sof_real);
-	if (m == NULL)
-		longjmp(buf, TS_MALLOC);
-	m[0] = 0.25f;
-	for (i = 1; i < len_m; i++)
-		m[i] = 1.f/(4 - m[i-1]);
-
-	/* forward sweep */
-	ts_arr_fill(_result_, n*dim, 0.f);
-	memcpy(_result_, points, sof_ctrlp);
-	memcpy(_result_+lst, points+lst, sof_ctrlp);
-	for (d = 0; d < dim; d++) {
-		k = dim+d;
-		_result_[k] = 6*points[k];
-		_result_[k] -= points[d];
-	}
-	for (i = 2; i <= n-2; i++) {
-		for (d = 0; d < dim; d++) {
-			j = (i-1)*dim+d;
-			k = i*dim+d;
-			l = (i+1)*dim+d;
-			_result_[k] = 6*points[k];
-			_result_[k] -= _result_[l];
-			_result_[k] -= m[i-2]*_result_[j]; /* i >= 2 */
-		}
-	}
-
-	/* back substitution */
-	if (n > 3)
-		ts_arr_fill(_result_+lst, dim, 0.f);
-	for (i = n-2; i >= 1; i--) {
-		for (d = 0; d < dim; d++) {
-			k = i*dim+d;
-			l = (i+1)*dim+d;
-			/* The following line is the reason why it's important
-			 * to not fill \p _result_ with 0 if n = 3. On the
-			 * other hand, if n > 3 subtracting 0 is exactly what
-			 * we want. */
-			_result_[k] -= _result_[l];
-			_result_[k] *= m[i-1]; /* i >= 1 */
-		}
-	}
-	if (n > 3)
-		memcpy(_result_+lst, points+lst, sof_ctrlp);
-
-	/* we are done */
-	free(m);
-}
-
-void ts_internal_relaxed_uniform_cubic_bspline(const tsReal *points, size_t n,
-	size_t dim, tsBSpline *_spline_, jmp_buf buf)
-{
-	const size_t order = 4;    /**< Order of spline to interpolate. */
-	const tsReal as = 1.f/6.f; /**< The value 'a sixth'. */
-	const tsReal at = 1.f/3.f; /**< The value 'a third'. */
-	const tsReal tt = 2.f/3.f; /**< The value 'two third'. */
-	size_t sof_ctrlp;          /**< Size of a single control point. */
-	const tsReal* b = points;  /**< Array of the b values. */
-	tsReal* s;                 /**< Array of the s values. */
-	size_t i, d;               /**< Used in for loops */
-	size_t j, k, l;            /**< Used as temporary indices. */
-
-	tsReal *ctrlp; /**< Pointer to the control points of \p _spline_. */
-
-	tsError e_;
-	jmp_buf b_;
-
-	/* input validation */
-	if (dim == 0)
-		longjmp(buf, TS_DIM_ZERO);
-	if (n <= 1)
-		longjmp(buf, TS_DEG_GE_NCTRLP);
-	/* in the following n >= 2 applies */
-
-	sof_ctrlp = dim * sizeof(tsReal); /* dim > 0 implies sof_ctrlp > 0 */
-
-	/* n >= 2 implies n-1 >= 1 implies (n-1)*4 >= 4 */
-	ts_internal_bspline_new(
-		(n-1)*4, dim, order-1, TS_BEZIERS, _spline_, buf);
-	ctrlp = ts_internal_bspline_access_ctrlp(_spline_);
-
-	TRY(b_, e_)
-		s = (tsReal*) malloc(n * sof_ctrlp);
-		if (!s)
-			longjmp(b_, TS_MALLOC);
-	CATCH
-		ts_bspline_free(_spline_);
-		longjmp(buf, e_);
-	ETRY
-
-	/* set s_0 to b_0 and s_n = b_n */
-	memcpy(s, b, sof_ctrlp);
-	memcpy(s + (n-1)*dim, b + (n-1)*dim, sof_ctrlp);
-
-	/* set s_i = 1/6*b_i + 2/3*b_{i-1} + 1/6*b_{i+1}*/
-	for (i = 1; i < n-1; i++) {
-		for (d = 0; d < dim; d++) {
-			j = (i-1)*dim+d;
-			k = i*dim+d;
-			l = (i+1)*dim+d;
-			s[k] = as * b[j];
-			s[k] += tt * b[k];
-			s[k] += as * b[l];
-		}
-	}
-
-	/* create beziers from b and s */
-	for (i = 0; i < n-1; i++) {
-		for (d = 0; d < dim; d++) {
-			j = i*dim+d;
-			k = i*4*dim+d;
-			l = (i+1)*dim+d;
-			ctrlp[k] = s[j];
-			ctrlp[k+dim] = tt*b[j] + at*b[l];
-			ctrlp[k+2*dim] = at*b[j] + tt*b[l];
-			ctrlp[k+3*dim] = s[l];
-		}
-	}
-
-	/* we are done */
-	free(s);
-}
-
-void ts_internal_bspline_interpolate_cubic(const tsReal *points, size_t n,
-	size_t dim, tsBSpline *_spline_, jmp_buf buf)
-{
-	tsError e;
-	jmp_buf b;
-
-	tsReal* thomas = (tsReal*) malloc(n*dim*sizeof(tsReal));
-	if (!thomas)
-		longjmp(buf, TS_MALLOC);
-
-	TRY(b, e)
-		ts_internal_bspline_thomas_algorithm(
-			points, n, dim, thomas, b);
-		ts_internal_relaxed_uniform_cubic_bspline(
-			thomas, n, dim, _spline_, b);
-	ETRY
-
-	free(thomas);
-	if (e < 0)
-		longjmp(buf, e);
-}
-
-tsError ts_bspline_interpolate_cubic(const tsReal *points, size_t n,
-	size_t dim, tsBSpline *_spline_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_interpolate_cubic(
-			points, n, dim, _spline_, buf);
-	CATCH
-		ts_bspline_default(_spline_);
-	ETRY
-	return err;
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Query Functions                                                          *
-*                                                                             *
-******************************************************************************/
-void ts_internal_bspline_eval(const tsBSpline *spline, tsReal u,
-	tsDeBoorNet *_deBoorNet_, jmp_buf buf)
-{
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t order = ts_bspline_order(spline);
-	const size_t dim = ts_bspline_dimension(spline);
-	const size_t num_knots = ts_bspline_num_knots(spline);
-	const size_t sof_ctrlp = dim * sizeof(tsReal);
-
-	const tsReal *ctrlp = ts_internal_bspline_access_ctrlp(spline);
-	const tsReal *knots = ts_internal_bspline_access_knots(spline);
-	tsReal *points;  /**< Pointer to the points of \p _deBoorNet_. */
-
-	size_t k;        /**< Index of \p u. */
-	size_t s;        /**< Multiplicity of \p u. */
-
-	tsReal uk;       /**< The actual used u. */
-	size_t from;     /**< Offset used to copy values. */
-	size_t fst;      /**< First affected control point, inclusive. */
-	size_t lst;      /**< Last affected control point, inclusive. */
-	size_t N;        /**< Number of affected control points. */
-
-	/* The following indices are used to create the DeBoor net. */
-	size_t lidx;     /**< Current left index. */
-	size_t ridx;     /**< Current right index. */
-	size_t tidx;     /**< Current to index. */
-	size_t r, i, d;  /**< Used in for loop. */
-	tsReal ui;       /**< Knot value at index i. */
-	tsReal a, a_hat; /**< Weighting factors of control points. */
-
-	/* Setup the net with its default values. */
-	ts_internal_deboornet_new(spline, _deBoorNet_, buf);
-	points = ts_internal_deboornet_access_points(_deBoorNet_);
-
-	/* 1. Find index k such that u is in between [u_k, u_k+1).
-	 * 2. Setup already known values.
-	 * 3. Decide by multiplicity of u how to calculate point P(u). */
-
-	/* 1. */
-	ts_internal_bspline_find_u(spline, u, &k, &s, buf);
-
-	/* 2. */
-	uk = knots[k];          /* Ensures that with any precision of  */
-	_deBoorNet_->pImpl->u = /* tsReal the knot vector stays valid. */
-		ts_fequals(u, uk) ? uk : u;
-	_deBoorNet_->pImpl->k = k;
-	_deBoorNet_->pImpl->s = s;
-        _deBoorNet_->pImpl->h = deg < s ? 0 : deg-s; /* prevent underflow */
-
-	/* 3. (by 1. s <= order)
-	 *
-	 * 3a) Check for s = order.
-	 *     Take the two points k-s and k-s + 1. If one of
-	 *     them doesn't exist, take only the other.
-	 * 3b) Use de boor algorithm to find point P(u). */
-	if (s == order) {
-		/* only one of the two control points exists */
-		if (k == deg || /* only the first */
-		    k == num_knots - 1) { /* only the last */
-			from = k == deg ? 0 : (k-s) * dim;
-			_deBoorNet_->pImpl->n_points = 1;
-			memcpy(points, ctrlp + from, sof_ctrlp);
-		} else {
-			from = (k-s) * dim;
-			_deBoorNet_->pImpl->n_points = 2;
-			memcpy(points, ctrlp + from, 2 * sof_ctrlp);
-		}
-	} else { /* by 3a) s <= deg (order = deg+1) */
-		fst = k-deg; /* by 1. k >= deg */
-		lst = k-s; /* s <= deg <= k */
-		N = lst-fst + 1; /* lst <= fst implies N >= 1 */
-
-		_deBoorNet_->pImpl->n_points = (size_t)(N * (N+1) * 0.5f);
-
-		/* copy initial values to output */
-		memcpy(points, ctrlp + fst*dim, N * sof_ctrlp);
-
-		lidx = 0;
-		ridx = dim;
-		tidx = N*dim; /* N >= 1 implies tidx > 0 */
-		r = 1;
-		for (;r <= ts_deboornet_num_insertions(_deBoorNet_); r++) {
-			i = fst + r;
-			for (; i <= lst; i++) {
-				ui = knots[i];
-				a = (ts_deboornet_knot(_deBoorNet_) - ui) /
-					(knots[i+deg-r+1] - ui);
-				a_hat = 1.f-a;
-
-				for (d = 0; d < dim; d++) {
-					points[tidx++] =
-						a_hat * points[lidx++] +
-						a     * points[ridx++];
-				}
-			}
-			lidx += dim;
-			ridx += dim;
-		}
-	}
-}
-
-tsError ts_bspline_eval(const tsBSpline *spline, tsReal u,
-	tsDeBoorNet *_deBoorNet_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_eval(spline, u, _deBoorNet_, buf);
-	CATCH
-		ts_deboornet_default(_deBoorNet_);
-	ETRY
-	return err;
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Transformation Functions                                                 *
-*                                                                             *
-******************************************************************************/
-void ts_internal_bspline_derive(const tsBSpline *spline,
-	tsBSpline *_derivative_, jmp_buf buf)
-{
-	const size_t sof_real = sizeof(tsReal);
-	const size_t dim = ts_bspline_dimension(spline);
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t num_ctrlp = ts_bspline_num_control_points(spline);
-	const size_t num_knots = ts_bspline_num_knots(spline);
-
-	tsBSpline tmp;  /**< Temporarily stores the result. */
-	const tsReal* from_ctrlp = ts_internal_bspline_access_ctrlp(spline);
-	const tsReal* from_knots = ts_internal_bspline_access_knots(spline);
-	tsReal* to_ctrlp = NULL; /**< Pointer to the control points of tmp. */
-	tsReal* to_knots = NULL; /**< Pointer to the knots of tmp. */
-
-	size_t i, j, k; /**< Used in for loops. */
-
-	if (deg < 1 || num_ctrlp < 2)
-		longjmp(buf, TS_UNDERIVABLE);
-
-	ts_internal_bspline_new(num_ctrlp-1, dim, deg-1, TS_NONE, &tmp, buf);
-	to_ctrlp = ts_internal_bspline_access_ctrlp(&tmp);
-	to_knots = ts_internal_bspline_access_knots(&tmp);
-
-	for (i = 0; i < num_ctrlp-1; i++) {
-		for (j = 0; j < dim; j++) {
-			if (ts_fequals(from_knots[i+deg+1], from_knots[i+1])) {
-				ts_bspline_free(&tmp);
-				longjmp(buf, TS_UNDERIVABLE);
-			} else {
-				k = i*dim + j;
-				to_ctrlp[k] = from_ctrlp[(i+1)*dim + j] - from_ctrlp[k];
-				to_ctrlp[k] *= deg;
-				to_ctrlp[k] /= from_knots[i+deg+1] - from_knots[i+1];
-			}
-		}
-	}
-	memcpy(to_knots, from_knots+1, (num_knots-2)*sof_real);
-
-	if (spline == _derivative_)
-		ts_bspline_free(_derivative_);
-	ts_bspline_move(&tmp, _derivative_);
-}
-
-tsError ts_bspline_derive(const tsBSpline *spline, tsBSpline *_derivative_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_derive(spline, _derivative_, buf);
-	CATCH
-		if (spline != _derivative_)
-			ts_bspline_default(_derivative_);
-	ETRY
-	return err;
-}
-
-void ts_internal_bspline_fill_knots(const tsBSpline *spline,
-	tsBSplineType type, tsReal min, tsReal max, tsBSpline *_result_,
-	jmp_buf buf)
-{
-	const size_t n_knots = ts_bspline_num_knots(spline);
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t order = deg + 1; /**< ensures order >= 1. */
-	tsReal fac; /**< Factor used to calculate the knot values. */
-	size_t i; /**< Used in for loops. */
-
-	tsReal *knots; /**< Pointer to the knots of \p _result_. */
-
-	/* order >= 1 implies 2*order >= 2 implies n_knots >= 2 */
-	if (n_knots < 2*order)
-		longjmp(buf, TS_DEG_GE_NCTRLP);
-	if (type == TS_BEZIERS && n_knots % order != 0)
-		longjmp(buf, TS_NUM_KNOTS);
-	if (min > max || ts_fequals(min, max))
-		longjmp(buf, TS_KNOTS_DECR);
-
-	/* copy spline even if type is TS_NONE */
-	ts_internal_bspline_copy(spline, _result_, buf);
-	knots = ts_internal_bspline_access_knots(_result_);
-
-	if (type == TS_OPENED) {
-		/* ensures that the first knot value is exactly \min */
-		knots[0] = min; /* n_knots >= 2 */
-
-		fac = (max-min) / (n_knots-1); /* n_knots >= 2 */
-		for (i = 1; i < n_knots-1; i++)
-			knots[i] = min + i*fac;
-
-		/* ensure that the last knot value is exactly \max */
-		knots[i] = max;
-	} else if (type == TS_CLAMPED) {
-		/* n_knots >= 2*order == 2*(deg+1) == 2*deg + 2 > 2*deg - 1 */
-		fac = (max-min) / (n_knots - 2*deg - 1);
-
-		ts_arr_fill(knots, order, min);
-		for (i = order ;i < n_knots-order; i++)
-			knots[i] = min + (i-deg)*fac;
-		ts_arr_fill(knots + i, order, max);
-	} else if (type == TS_BEZIERS) {
-		/* n_knots >= 2*order implies n_knots/order >= 2 */
-		fac = (max-min) / (n_knots/order - 1);
-
-		ts_arr_fill(knots, order, min);
-		for (i = order; i < n_knots-order; i += order)
-			ts_arr_fill(knots + i,
-				    order, min + (i/order)*fac);
-		ts_arr_fill(knots + i, order, max);
-	}
-}
-
-tsError ts_bspline_fill_knots(const tsBSpline *spline, tsBSplineType type,
-	tsReal min, tsReal max, tsBSpline *_result_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_fill_knots(
-			spline, type, min, max, _result_, buf);
-	CATCH
-		if (spline != _result_)
-			ts_bspline_default(_result_);
-	ETRY
-	return err;
-}
-
-void ts_internal_bspline_resize(const tsBSpline *spline, int n, int back,
-	tsBSpline *_resized_, jmp_buf buf)
-{
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t dim = ts_bspline_dimension(spline);
-	const size_t sof_real = sizeof(tsReal);
-
-	const size_t num_ctrlp = ts_bspline_num_control_points(spline);
-	const size_t num_knots = ts_bspline_num_knots(spline);
-	const size_t nnum_ctrlp = num_ctrlp + n; /**< New length of ctrlp. */
-	const size_t nnum_knots = num_knots + n; /**< New length of knots. */
-	const size_t min_num_ctrlp_vec = n < 0 ? nnum_ctrlp : num_ctrlp;
-	const size_t min_num_knots_vec = n < 0 ? nnum_knots : num_knots;
-
-	const size_t sof_min_num_ctrlp = min_num_ctrlp_vec * dim * sof_real;
-	const size_t sof_min_num_knots = min_num_knots_vec * sof_real;
-
-	tsBSpline tmp; /**< Temporarily stores the result. */
-	const tsReal* from_ctrlp = ts_internal_bspline_access_ctrlp(spline);
-	const tsReal* from_knots = ts_internal_bspline_access_knots(spline);
-	tsReal* to_ctrlp = NULL; /**< Pointer to the control points of tmp. */
-	tsReal* to_knots = NULL; /**< Pointer to the knots of tmp. */
-
-	/* If n is 0 the spline must not be resized. */
-	if (n == 0) {
-		ts_internal_bspline_copy(spline, _resized_, buf);
-		return;
-	}
-
-	ts_internal_bspline_new(nnum_ctrlp, dim, deg, TS_NONE, &tmp, buf);
-	to_ctrlp = ts_internal_bspline_access_ctrlp(&tmp);
-	to_knots = ts_internal_bspline_access_knots(&tmp);
-
-	/* Copy control points and knots. */
-	if (!back && n < 0) {
-		memcpy(to_ctrlp, from_ctrlp + n*dim, sof_min_num_ctrlp);
-		memcpy(to_knots, from_knots + n    , sof_min_num_knots);
-	} else if (!back && n > 0) {
-		memcpy(to_ctrlp + n*dim, from_ctrlp, sof_min_num_ctrlp);
-		memcpy(to_knots + n    , from_knots, sof_min_num_knots);
-	} else {
-		/* n != 0 implies back == true */
-		memcpy(to_ctrlp, from_ctrlp, sof_min_num_ctrlp);
-		memcpy(to_knots, from_knots, sof_min_num_knots);
-	}
-
-	if (spline == _resized_)
-		ts_bspline_free(_resized_);
-	ts_bspline_move(&tmp, _resized_);
-}
-
-tsError ts_bspline_resize(const tsBSpline *spline, int n, int back,
-	tsBSpline *_resized_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_resize(spline, n, back, _resized_, buf);
-	CATCH
-		if (spline != _resized_)
-			ts_bspline_default(_resized_);
-	ETRY
-	return err;
-}
-
-void ts_internal_bspline_insert_knot(const tsBSpline *spline,
-	const tsDeBoorNet *deBoorNet, size_t n, tsBSpline *_result_,
-	jmp_buf buf)
-{
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t dim = ts_bspline_dimension(spline);
-	const tsReal *ctrlp_spline = ts_internal_bspline_access_ctrlp(spline);
-	const tsReal *knots_spline = ts_internal_bspline_access_knots(spline);
-	const size_t k = ts_deboornet_index(deBoorNet);
-	const size_t s = ts_deboornet_multiplicity(deBoorNet);
-	const size_t sof_real = sizeof(tsReal);
-	const size_t sof_ctrlp = dim * sof_real;
-
-	size_t N;     /**< Number of affected control points. */
-	tsReal* from; /**< Pointer to copy the values from. */
-	tsReal* to;   /**< Pointer to copy the values to. */
-	int stride;   /**< Stride of the next pointer to copy. */
-	size_t i;     /**< Used in for loops. */
-
-	tsReal *ctrlp_result;
-	tsReal *knots_result;
-	size_t num_ctrlp_result;
-	size_t num_knots_result;
-
-	if (s+n > ts_bspline_order(spline)) {
-		longjmp(buf, TS_MULTIPLICITY);
-	}
-
-	/* Use ::ts_bspline_resize even if \p n is 0 to copy the spline if
-	 * necessary. */
-	ts_internal_bspline_resize(spline, (int)n, 1, _result_, buf);
-	if (n == 0) /* Nothing to insert. */
-		return;
-	ctrlp_result = ts_internal_bspline_access_ctrlp(_result_);
-	knots_result = ts_internal_bspline_access_knots(_result_);
-	num_ctrlp_result = ts_bspline_num_control_points(_result_);
-	num_knots_result = ts_bspline_num_knots(_result_);
-
-	/* n > 0 implies s <= deg implies a regular evaluation implies h+1 is
-	 * valid. */
-	N = ts_deboornet_num_insertions(deBoorNet) + 1;
-
-	/* 1. Copy all necessary control points and knots from
-	 *    the original B-Spline.
-	 * 2. Copy all necessary control points and knots from
-	 *    the de Boor net. */
-
-	/* 1.
-	 *
-	 * a) Copy left hand side control points from original b-spline.
-	 * b) Copy right hand side control points from original b-spline.
-	 * c) Copy left hand side knots from original b-spline.
-	 * d) Copy right hand side knots form original b-spline. */
-	/* copy control points */
-	memmove(ctrlp_result, ctrlp_spline, (k-deg) * sof_ctrlp);      /* a) */
-	from = (tsReal *) ctrlp_spline + dim*(k-deg+N);
-	/* n >= 0 implies to >= from */
-	to = ctrlp_result + dim*(k-deg+N+n);
-	memmove(to, from, (num_ctrlp_result-n-(k-deg+N)) * sof_ctrlp); /* b) */
-	/* copy knots */
-	memmove(knots_result, knots_spline, (k+1) * sof_real);         /* c) */
-	from = (tsReal *) knots_spline + k+1;
-	/* n >= 0 implies to >= from */
-	to = knots_result + k+1+n;
-	memmove(to, from, (num_knots_result-n-(k+1)) * sof_real);      /* d) */
-
-	/* 2.
-	 *
-	 * a) Copy left hand side control points from de boor net.
-	 * b) Copy middle part control points from de boor net.
-	 * c) Copy right hand side control points from de boor net.
-	 * d) Insert knots with u_k. */
-	from = ts_internal_deboornet_access_points(deBoorNet);
-	to = ctrlp_result + (k-deg)*dim;
-	stride = (int)(N*dim);
-
-	/* copy control points */
-	for (i = 0; i < n; i++) {                                      /* a) */
-		memcpy(to, from, sof_ctrlp);
-		from += stride;
-		to += dim;
-		stride -= (int)dim;
-	}
-	memcpy(to, from, (N-n) * sof_ctrlp);                           /* b) */
-
-	from -= dim;
-	to += (N-n)*dim;
-	/* N = h+1 with h = deg-s (ts_internal_bspline_eval) implies
-	 * N = deg-s+1 = order-s. n <= order-s implies
-	 * N-n+1 >= order-s - order-s + 1 = 1. Thus, -(int)(N-n+1) <= -1. */
-	stride = -(int)(N-n+1) * (int)dim;
-
-	for (i = 0; i < n; i++) {                                      /* c) */
-		memcpy(to, from, sof_ctrlp);
-		from += stride;
-		stride -= (int)dim;
-		to += dim;
-	}
-	/* copy knots */
-	to = knots_result + k+1;
-	for (i = 0; i < n; i++) {                                      /* d) */
-		*to = ts_deboornet_knot(deBoorNet);
-		to++;
-	}
-}
-
-tsError ts_bspline_insert_knot(const tsBSpline *spline, tsReal u, size_t n,
-	tsBSpline *_result_, size_t* k)
-{
-	tsDeBoorNet net;
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_eval(spline, u, &net, buf);
-		ts_internal_bspline_insert_knot(
-			spline, &net, n, _result_, buf);
-		*k = ts_deboornet_index(&net) + n;
-	CATCH
-		if (spline != _result_)
-			ts_bspline_default(_result_);
-		*k = 0;
-	ETRY
-
-	ts_deboornet_free(&net);
-	return err;
-}
-
-void ts_internal_bspline_split(const tsBSpline *spline, tsReal u,
-	tsBSpline *_split_, size_t* k, jmp_buf buf)
-{
-	tsDeBoorNet net;
-	tsError e;
-	jmp_buf b;
-
-	TRY(b, e)
-		ts_internal_bspline_eval(spline, u, &net, b);
-		if (ts_deboornet_multiplicity(&net)
-		    		== ts_bspline_order(spline)) {
-			ts_internal_bspline_copy(spline, _split_, b);
-			*k = ts_deboornet_index(&net);
-		} else {
-			ts_internal_bspline_insert_knot(spline, &net,
-				ts_deboornet_num_insertions(&net) + 1,
-				_split_, b);
-			*k = ts_deboornet_index(&net) +
-				ts_deboornet_num_insertions(&net) + 1;
-		}
-	CATCH
-		*k = 0;
-	ETRY
-
-	ts_deboornet_free(&net);
-	if (e < 0)
-		longjmp(buf, e);
-}
-
-tsError ts_bspline_split(const tsBSpline *spline, tsReal u, tsBSpline *_split_,
-	size_t* k)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_split(spline, u, _split_, k, buf);
-	CATCH
-		if (spline != _split_)
-			ts_bspline_default(_split_);
-	ETRY
-	return err;
-}
-
-void ts_internal_bspline_buckle(const tsBSpline *spline, tsReal b,
-	tsBSpline *_buckled_, jmp_buf buf)
-{
-	const tsReal b_hat  = 1.f-b; /**< The straightening factor. */
-	const size_t dim = ts_bspline_dimension(spline);
-	const size_t N = ts_bspline_num_control_points(spline);
-	const tsReal* p0 = ts_internal_bspline_access_ctrlp(spline);
-	const tsReal* pn_1 = p0 + (N-1)*dim;
-
-	tsReal *ctrlp; /**< Pointer to the control points of \p _buckled_. */
-	size_t i, d; /**< Used in for loops. */
-
-	ts_internal_bspline_copy(spline, _buckled_, buf);
-	ctrlp = ts_internal_bspline_access_ctrlp(_buckled_);
-
-	for (i = 0; i < N; i++) {
-		for (d = 0; d < dim; d++) {
-			ctrlp[i*dim + d] =
-				b     * ctrlp[i*dim + d] +
-				b_hat * (p0[d] + ((tsReal)i / (N-1)) * (pn_1[d] - p0[d]));
-		}
-	}
-}
-
-tsError ts_bspline_buckle(const tsBSpline *spline, tsReal b,
-	tsBSpline *_buckled_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_buckle(spline, b, _buckled_, buf);
-	CATCH
-		if (spline != _buckled_)
-			ts_bspline_default(_buckled_);
-	ETRY
-	return err;
-}
-
-void ts_internal_bspline_to_beziers(const tsBSpline *spline,
-	tsBSpline *_beziers_, jmp_buf buf)
-{
-	const size_t deg = ts_bspline_degree(spline);
-	const size_t order = ts_bspline_order(spline);
-
-	int resize;    /**< Number of control points to add/remove. */
-	size_t k;      /**< Index of the split knot value. */
-	tsReal u_min;  /**< Minimum of the knot values. */
-	tsReal u_max;  /**< Maximum of the knot values. */
-
-	tsBSpline tmp;    /**< Temporarily stores the result. */
-	tsReal *knots;    /**< Pointer to the knots of tmp. */
-	size_t num_knots; /**< Number of knots in tmp. */
-
-	tsError e;
-	jmp_buf b;
-
-	ts_internal_bspline_copy(spline, &tmp, buf);
-	knots = ts_internal_bspline_access_knots(&tmp);
-	num_knots = ts_bspline_num_knots(&tmp);
-
-	TRY(b, e)
-		/* DO NOT FORGET TO UPDATE knots AND num_knots AFTER EACH
-		 * TRANSFORMATION OF tmp! */
-
-		/* fix first control point if necessary */
-		u_min = knots[deg];
-		if (!ts_fequals(knots[0], u_min)) {
-			ts_internal_bspline_split(&tmp, u_min, &tmp, &k, b);
-			resize = (int)(-1*deg + (deg*2 - k));
-			ts_internal_bspline_resize(&tmp, resize, 0, &tmp, b);
-			knots = ts_internal_bspline_access_knots(&tmp);
-			num_knots = ts_bspline_num_knots(&tmp);
-		}
-
-		/* fix last control point if necessary */
-		u_max = knots[num_knots - order];
-		if (!ts_fequals(knots[num_knots - 1], u_max)) {
-			ts_internal_bspline_split(&tmp, u_max, &tmp, &k, b);
-			num_knots = ts_bspline_num_knots(&tmp);
-			resize = (int)(-1*deg + (k - (num_knots - order)));
-			ts_internal_bspline_resize(&tmp, resize, 1, &tmp, b);
-			knots = ts_internal_bspline_access_knots(&tmp);
-			num_knots = ts_bspline_num_knots(&tmp);
-		}
-
-		k = order;
-		while (k < num_knots - order) {
-			ts_internal_bspline_split(&tmp, knots[k], &tmp, &k, b);
-			knots = ts_internal_bspline_access_knots(&tmp);
-			num_knots = ts_bspline_num_knots(&tmp);
-			k++;
-		}
-
-		if (spline == _beziers_)
-			ts_bspline_free(_beziers_);
-		ts_bspline_move(&tmp, _beziers_);
-	ETRY
-
-	ts_bspline_free(&tmp);
-	if (e < 0)
-		longjmp(buf, e);
-}
-
-tsError ts_bspline_to_beziers(const tsBSpline *spline, tsBSpline *_beziers_)
-{
-	tsError err;
-	jmp_buf buf;
-	TRY(buf, err)
-		ts_internal_bspline_to_beziers(spline, _beziers_, buf);
-	CATCH
-		if (spline != _beziers_)
-			ts_bspline_default(_beziers_);
-	ETRY
-	return err;
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Utility Functions                                                        *
-*                                                                             *
-******************************************************************************/
-int ts_fequals(tsReal x, tsReal y)
-{
-	if (fabs(x-y) <= FLT_MAX_ABS_ERROR) {
-		return 1;
-	} else {
-		const tsReal r = (tsReal)fabs(x) > (tsReal)fabs(y) ?
-			(tsReal)fabs((x-y) / x) : (tsReal)fabs((x-y) / y);
-		return r <= FLT_MAX_REL_ERROR;
-	}
-}
-
-const char* ts_enum_str(tsError err)
-{
-	if (err == TS_MALLOC)
-		return "malloc failed";
-	else if (err == TS_DIM_ZERO)
-		return "dim == 0";
-	else if (err == TS_DEG_GE_NCTRLP)
-		return "deg >= #ctrlp";
-	else if (err == TS_U_UNDEFINED)
-		return "spline is undefined at given u";
-	else if (err == TS_MULTIPLICITY)
-		return "s > order";
-	else if (err == TS_KNOTS_DECR)
-		return "decreasing knot vector";
-	else if (err == TS_NUM_KNOTS)
-		return "unexpected number of knots";
-	else if (err == TS_UNDERIVABLE)
-		return "spline is not derivable";
-	return "unknown error";
-}
-
-tsError ts_str_enum(const char *str)
-{
-	if (!strcmp(str, ts_enum_str(TS_MALLOC)))
-		return TS_MALLOC;
-	else if (!strcmp(str, ts_enum_str(TS_DIM_ZERO)))
-		return TS_DIM_ZERO;
-	else if (!strcmp(str, ts_enum_str(TS_DEG_GE_NCTRLP)))
-		return TS_DEG_GE_NCTRLP;
-	else if (!strcmp(str, ts_enum_str(TS_U_UNDEFINED)))
-		return TS_U_UNDEFINED;
-	else if (!strcmp(str, ts_enum_str(TS_MULTIPLICITY)))
-		return TS_MULTIPLICITY;
-	else if (!strcmp(str, ts_enum_str(TS_KNOTS_DECR)))
-		return TS_KNOTS_DECR;
-	else if (!strcmp(str, ts_enum_str(TS_NUM_KNOTS)))
-		return TS_NUM_KNOTS;
-	else if (!strcmp(str, ts_enum_str(TS_UNDERIVABLE)))
-		return TS_UNDERIVABLE;
-	return TS_SUCCESS;
-}
-
-void ts_arr_fill(tsReal *arr, size_t num, tsReal val)
-{
-	size_t i;
-	for (i = 0; i < num; i++)
-		arr[i] = val;
-}
-
-tsReal ts_ctrlp_dist2(const tsReal *x, const tsReal *y, size_t dim)
-{
-	tsReal sum = 0;
-	size_t i;
-	for (i = 0; i < dim; i++)
-		sum += (x[i] - y[i]) * (x[i] - y[i]);
-	return (tsReal) sqrt(sum);
-}

src/decition/decition_brain/decision/tinyspline/tinyspline.h

@@ -1,1152 +0,0 @@
-/** @file */
-
-#ifndef TINYSPLINE_H
-#define	TINYSPLINE_H
-
-#include <stddef.h>
-
-#ifdef	__cplusplus
-extern "C" {
-#endif
-
-/******************************************************************************
-*                                                                             *
-* :: System Dependent Configuration                                           *
-*                                                                             *
-* The following configuration values must be adapted to your system. Some of  *
-* them may be configured with preprocessor definitions. The default values    *
-* should be fine for most modern hardware, such as x86, x86_64, and arm.      *
-*                                                                             *
-******************************************************************************/
-#ifdef TINYSPLINE_FLOAT_PRECISION
-typedef float tsReal;
-#else
-typedef double tsReal;
-#endif
-
-#define FLT_MAX_ABS_ERROR 1e-5
-#define FLT_MAX_REL_ERROR 1e-8
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Data Types                                                               *
-*                                                                             *
-* The following section defines all data types available in TinySpline.       *
-*                                                                             *
-******************************************************************************/
-/**
- * Defines the error codes used by various functions to indicate different
- * types of errors. The following code snippet shows how to handle errors:
- *
- *      tsError err = ...                  // any function call here
- *      if (err) {                         // or use err != TS_SUCCESS
- *          printf("we got an error!");
- *          return err;                    // you may want to reuse error codes
- *      }
- */
-typedef enum
-{
-	/* No error. */
-	TS_SUCCESS = 0,
-
-	/* Unable to allocate memory (using malloc/realloc). */
-	TS_MALLOC = -1,
-
-	/* The dimension of the control points are 0. */
-	TS_DIM_ZERO = -2,
-
-	/* Degree of spline >= number of control points. */
-	TS_DEG_GE_NCTRLP = -3,
-
-	/* Spline is not defined at knot value u. */
-	TS_U_UNDEFINED = -4,
-
-	/* Multiplicity of a knot (s) > order of spline.  */
-	TS_MULTIPLICITY = -5,
-
-	/* Decreasing knot vector. */
-	TS_KNOTS_DECR = -6,
-
-	/* Unexpected number of knots. */
-	TS_NUM_KNOTS = -7,
-
-	/* Spline is not derivable */
-	TS_UNDERIVABLE = -8,
-
-	/* len_control_points % dim != 0 */
-	TS_LCTRLP_DIM_MISMATCH = -10
-} tsError;
-
-/**
- * Describes the structure of the knot vector of a NURBS/B-Spline. For more
- * details, see:
- *
- *     www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-curve.html
- */
-typedef enum
-{
-	/* Not available/Undefined. */
-	TS_NONE = 0,
-
-	/* Uniformly spaced knot vector. */
-	TS_OPENED = 1,
-
-	/* Uniformly spaced knot vector with clamped end knots. */
-	TS_CLAMPED = 2,
-
-	/* Uniformly spaced knot vector with s(u) = order of spline. */
-	TS_BEZIERS = 3
-} tsBSplineType;
-
-/**
- * Represents a B-Spline which may also be used for NURBS, Bezier curves,
- * lines, and points. NURBS are represented by homogeneous coordinates where
- * the last component of a control point stores the weight of this control
- * point. Bezier curves are B-Splines with 'num_control_points == order' and a
- * clamped knot vector, therefore passing through their first and last control
- * point (a property which does not necessarily apply to B-Splines and NURBS).
- * If a Bezier curve consists of two control points only, we call it a line.
- * Points, ultimately, are just very short lines consisting of a single control
- * point.
- *
- * Two dimensional control points are stored as follows:
- *
- *     [x_0, y_0, x_1, y_1, ..., x_n-1, y_n-1]
- *
- * Tree dimensional control points are stored as follows:
- *
- *     [x_0, y_0, z_0, x_1, y_1, z_1, ..., x_n-1, y_n-1, z_n-1]
- *
- * ... and so on. NURBS are represented by homogeneous coordinates. For
- * example, let's say we have a NURBS in 2D consisting of 11 control points
- * where 'w_i' is the weight of the i'th control point. Then, the corresponding
- * control points are stored as follows:
- *
- *     [x_0*w_0, y_0*w_0, w_0, x_1*w_1, y_1*w_1, w_1, ..., x_10*w_10, y_10*w_10, w_10]
- */
-typedef struct
-{
-	struct tsBSplineImpl *pImpl; /**< The actual implementation. */
-} tsBSpline;
-
-/**
- * Represents the output of De Boor's algorithm. De Boor's algorithm is used to
- * evaluate a spline at given knot value 'u' by iteratively computing a net of
- * intermediate values until the result is available:
- *
- *     https://en.wikipedia.org/wiki/De_Boor%27s_algorithm
- *     https://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/de-Boor.html
- *
- * All points of a net are stored in 'points'. The resulting point is the last
- * point in 'points' and, for the sake of convenience, may be accessed with
- * 'result':
- *
- *     tsDeBoorNet net = ...    // evaluate an arbitrary spline and store
- *                              // the resulting net of points in 'net'
- *
- *     ts_deboornet_result(...) // use 'result' to access the resulting point
- *
- * Two dimensional points are stored as follows:
- *
- *     [x_0, y_0, x_1, y_1, ..., x_n-1, y_n-1]
- *
- * Tree dimensional points are stored as follows:
- *
- *     [x_0, y_0, z_0, x_1, y_1, z_1, ..., x_n-1, y_n-1, z_n-1]
- *
- * ... and so on. The output also supports homogeneous coordinates described
- * above.
- *
- * There is a special case in which the evaluation of a knot value 'u' returns
- * two results instead of one. It occurs when the multiplicity of 'u' ( s(u) )
- * is equals to a spline's order, indicating that the spline is discontinuous
- * at 'u'. This is common practice for B-Splines (and NURBS) consisting of
- * connected Bezier curves where the endpoint of curve 'c_i' is equals to the
- * start point of curve 'c_i+1'. The end point of 'c_i' and the start point of
- * 'c_i+1' may still be completely different though, yielding to a spline
- * having a (real and visible) gap at 'u'. Consequently, De Boor's algorithm
- * must return two results if 's(u) == order' in order to give access to the
- * desired points.  In such case, 'points' stores only the two resulting points
- * (there is no net to calculate) and 'result' points to the *first* point in
- * 'points'. Since having (real) gaps in splines is unusual, both points in
- * 'points' are generally equals, making it easy to handle this special case by
- * accessing 'result' as already shown above for regular cases:
- *
- *     tsDeBoorNet net = ...    // evaluate a spline which is discontinuous at
- *                              // the given knot value, yielding to a net with
- *                              // two results
- *
- *     ts_deboornet_result(...) // use 'result' to access the resulting point
- *
- * However, you can access both points if necessary:
- *
- *     tsDeBoorNet net = ...    // evaluate a spline which is discontinuous at
- *                              // the given knot value, yielding to a net with
- *                              // two results
- *
- *     ts_deboornet_result(...)[0] ...       // stores the first component of
- *                                           // the first point
- *
- *     ts_deboornet_result(...)[dim(spline)] // stores the first component of
- *                                           // the second point
- *
- * As if this wasn't complicated enough, there is an exception for this special
- * case, yielding to exactly one result (just like the regular case) even if
- * 's(u) == order'. It occurs when 'u' is the lower or upper bound of a
- * spline's domain. For instance, if 'b' is a spline with domain [0, 1] and is
- * evaluated at 'u = 0' or 'u = 1' then 'result' is *always* a single point
- * regardless of the multiplicity of 'u'. Hence, handling this exception is
- * straightforward:
- *
- *     tsDeBoorNet net = ...    // evaluate a spline at the lower or upper
- *                              // bound of its domain, for instance, 0 or 1
- *
- *     ts_deboornet_result(...) // use 'result' to access the resulting point
- *
- * In summary, we have three different types of evaluation. 1) The regular case
- * returning all points of the net we used to calculate the resulting point. 2)
- * The special case returning exactly two points which is required for splines
- * with (real) gaps. 3) The exception of 2) returning exactly one point even if
- * 's(u) == order'. All in all this looks quite complex (and actually it is)
- * but for most applications you do not have to deal with it. Just use 'result'
- * to access the outcome of De Boor's algorithm.
- */
-typedef struct
-{
-	struct tsDeBoorNetImpl *pImpl; /**< The actual implementation. */
-} tsDeBoorNet;
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Field Access Functions                                                   *
-*                                                                             *
-* The following section contains getter and setter functions for the internal *
-* state of the structs listed above.                                          *
-*                                                                             *
-******************************************************************************/
-/**
- * Returns the degree of \p spline.
- *
- * @param spline
- * 	The spline whose degree is read.
- * @return
- * 	The degree of \p spline.
- */
-size_t ts_bspline_degree(const tsBSpline *spline);
-
-/**
- * Sets the degree of \p spline.
- *
- * @param spline
- * 	The spline whose degree is set.
- * @param deg
- * 	The degree to be set.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_DEG_GE_NCTRLP
- * 	If \p degree >= ts_bspline_get_control_points(spline).
- */
-tsError ts_bspline_set_degree(tsBSpline *spline, size_t deg);
-
-/**
- * Returns the order (degree + 1) of \p spline.
- *
- * @param spline
- * 	The spline whose order is read.
- * @return
- * 	The order of \p spline.
- */
-size_t ts_bspline_order(const tsBSpline *spline);
-
-/**
- * Sets the order (degree + 1) of \p spline.
- *
- * @param spline
- * 	The spline whose order is set.
- * @param order
- * 	The order to be set.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_DEG_GE_NCTRLP
- * 	If \p order > ts_bspline_get_control_points(spline) or if \p order == 0
- * 	( due to the underflow resulting from: order - 1 => 0 - 1 => INT_MAX
- * 	which always is >= ts_bspline_get_control_points(spline) ).
- */
-tsError ts_bspline_set_order(tsBSpline *spline, size_t order);
-
-/**
- * Returns the dimension of \p spline. The dimension of a spline describes the
- * number of components for each point in ts_bspline_get_control_points(spline).
- * One-dimensional splines are possible, albeit their benefit might be
- * questionable.
- *
- * @param spline
- * 	The spline whose dimension is read.
- * @return
- * 	The dimension of \p spline.
- */
-size_t ts_bspline_dimension(const tsBSpline *spline);
-
-/**
- * Sets the dimension of \p spline. The following conditions must be satisfied:
- *
- * 	(1) dim >= 1
- * 	(2) len_control_points % dim == 0
- *
- * with _len_control_points_ being the length of the control point array of \p
- * spline. The dimension of a spline describes the number of components for
- * each point in ts_bspline_get_control_points(spline). One-dimensional splines
- * are possible, albeit their benefit might be questionable.
- *
- * @param spline
- * 	The spline whose dimension is set.
- * @param dim
- * 	The dimension to be set.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_DIM_ZERO
- * 	If \p dimension == 0.
- * @return TS_LCTRLP_DIM_MISMATCH
- * 	If len_control_points % \p dim != 0
- */
-tsError ts_bspline_set_dimension(tsBSpline *spline, size_t dim);
-
-/**
- * Returns the length of the control point array of \p spline.
- *
- * @param spline
- * 	The spline with its control point array whose length is read.
- * @return
- * 	The length of the control point array of \p spline.
- */
-size_t ts_bspline_len_control_points(const tsBSpline *spline);
-
-/**
- * Returns the number of control points of \p spline.
- *
- * @param spline
- * 	The spline whose number of control points is read.
- * @return
- * 	The number of control points of \p spline.
- */
-size_t ts_bspline_num_control_points(const tsBSpline *spline);
-
-/**
- * Returns the size of the control point array of \p spline. This function may
- * be useful when copying control points using memcpy or memmove.
- *
- * @param spline
- * 	The spline with its control point array whose size is read.
- * @return
- * 	The size of the control point array of \p spline.
- */
-size_t ts_bspline_sof_control_points(const tsBSpline *spline);
-
-/**
- * Returns a deep copy of the control points of \p spline.
- *
- * @param spline
- * 	The spline whose control points are read.
- * @param ctrlp
- * 	The output array.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_bspline_control_points(const tsBSpline *spline, tsReal **ctrlp);
-
-/**
- * Sets the control points of \p spline. Creates a deep copy of \p ctrlp.
- *
- * @param spline
- * 	The spline whose control points are set.
- * @param ctrlp
- * 	The values to deep copy.
- * @return TS_SUCCESS
- * 	On success.
- */
-tsError ts_bspline_set_control_points(tsBSpline *spline, const tsReal *ctrlp);
-
-/**
- * Returns the number of knots of \p spline.
- *
- * @param spline
- * 	The spline whose number of knots is read.
- * @return
- * 	The number of knots of \p spline.
- */
-size_t ts_bspline_num_knots(const tsBSpline *spline);
-
-/**
- * Returns the size of the knot array of \p spline. This function may be useful
- * when copying knots using memcpy or memmove.
- *
- * @param spline
- * 	The spline with its knot array whose size is read.
- * @return TS_SUCCESS
- * 	The size of the knot array of \p spline.
- */
-size_t ts_bspline_sof_knots(const tsBSpline *spline);
-
-/**
- * Returns a deep copy of the knots of \p spline.
- *
- * @param spline
- * 	The spline whose knots are read.
- * @param knots
- * 	The output array.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_bspline_knots(const tsBSpline *spline, tsReal **knots);
-
-/**
- * Sets the knots of \p spline. Creates a deep copy of \p knots.
- *
- * @param spline
- * 	The spline whose knots are set.
- * @param knots
- * 	The values to deep copy.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_KNOTS_DECR
- * 	If the knot vector is decreasing.
- * @return TS_MULTIPLICITY
- * 	If there is a knot with multiplicity > order
- */
-tsError ts_bspline_set_knots(tsBSpline *spline, const tsReal *knots);
-
-/* ------------------------------------------------------------------------- */
-
-/**
- * Returns the knot (sometimes also called 'u' or 't') of \p net.
- *
- * @param net
- * 	The net whose knot is read.
- * @return
- * 	The knot of \p net.
- */
-tsReal ts_deboornet_knot(const tsDeBoorNet *net);
-
-/**
- * Returns the index [u_k, u_k+1) with u being the knot of \p net.
- *
- * @param net
- * 	The net whose index is read.
- * @return
- * 	The index [u_k, u_k+1) with u being the knot of \p net.
- */
-size_t ts_deboornet_index(const tsDeBoorNet *net);
-
-/**
- * Returns the multiplicity of the knot of \p net.
- *
- * @param net
- * 	The net whose multiplicity is read.
- * @return
- * 	The multiplicity of the knot of \p net.
- */
-size_t ts_deboornet_multiplicity(const tsDeBoorNet *net);
-
-/**
- * Returns the number of insertion that were necessary to evaluate the knot of
- * \p net.
- *
- * @param net
- * 	The net with its knot whose number of insertions is read.
- * @return
- * 	The number of insertions that were necessary to evaluate the knot of \p
- * 	net.
- */
-size_t ts_deboornet_num_insertions(const tsDeBoorNet *net);
-
-/**
- * Returns the dimension of \p net. The dimension of a net describes the number
- * of components for each point in ts_bspline_get_points(spline).
- * One-dimensional nets are possible, albeit their benefit might be
- * questionable.
- *
- * @param net
- * 	The net whose dimension is read.
- * @return
- * 	The dimension of \p net.
- */
-size_t ts_deboornet_dimension(const tsDeBoorNet *net);
-
-/**
- * Returns the length of the point array of \p net.
- *
- * @param net
- * 	The net with its point array whose length is read.
- * @return
- * 	The length of the point array of \p net.
- */
-size_t ts_deboornet_len_points(const tsDeBoorNet *net);
-
-/**
- * Returns the number of points of \p net.
- *
- * @param net
- * 	The net whose number of points is read.
- * @return
- * 	The number of points of \p net.
- */
-size_t ts_deboornet_num_points(const tsDeBoorNet *net);
-
-/**
- * Returns the size of the point array of \p net. This function may be useful
- * when copying points using memcpy or memmove.
- *
- * @param net
- * 	The net with its point array whose size is read.
- * @return
- * 	The size of the point array of \p net.
- */
-size_t ts_deboornet_sof_points(const tsDeBoorNet *net);
-
-/**
- * Returns a deep copy of the points of \p net.
- *
- * @param net
- * 	The net whose points is read.
- * @param points
- * 	The output array.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_deboornet_points(const tsDeBoorNet *net, tsReal **points);
-
-/**
- * Returns the length of the result array of \p net.
- *
- * @param net
- * 	The net with its result array whose length is read.
- * @return
- * 	The length of the result array of \p net.
- */
-size_t ts_deboornet_len_result(const tsDeBoorNet *net);
-
-/**
- * Returns the number of points in the result array of \p net
- * (1 <= num_result <= 2).
- *
- * @param net
- * 	The net with its result array whose number of points is read.
- * @return
- * 	The number of points in the result array of \p net.
- */
-size_t ts_deboornet_num_result(const tsDeBoorNet *net);
-
-/**
- * Returns the size of the result array of \p net. This function may be useful
- * when copying results using memcpy or memmove.
- *
- * @param net
- * 	The net with its result array whose size is read.
- * @return TS_SUCCESS
- * 	The size of the result array of \p net.
- */
-size_t ts_deboornet_sof_result(const tsDeBoorNet *net);
-
-/**
- * Returns a deep copy of the result of \p net.
- *
- * @param net
- * 	The net whose result is read.
- * @param result
- * 	The output array.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_deboornet_result(const tsDeBoorNet *net, tsReal **result);
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Constructors, Destructors, Copy, and Move Functions                      *
-*                                                                             *
-* The following section contains functions to create and delete instances of  *
-* the data types listed above. Additionally, each data type has a copy and    *
-* move function.                                                              *
-*                                                                             *
-******************************************************************************/
-/**
- * The default constructor of tsBSpline.
- *
- * All values of \p \_spline\_ are set to 0/NULL.
- * 
- * @param \_spline\_
- * 	The spline whose values are set 0/NULL.
- */
-void ts_bspline_default(tsBSpline *_spline_);
-
-/**
- * A convenient constructor for tsBSpline.
- *
- * On error, all values of \p \_spline\_ are set to 0/NULL.
- *
- * @param n_ctrlp
- * 	The number of control points of \p \_spline\_.
- * @param dim
- * 	The dimension of each control point in \p \_spline\_.
- * @param deg
- * 	The degree of \p \_spline\_.
- * @param type
- * 	How to setup the knot vector of \p \_spline\_.
- * @param \_spline\_
- * 	The output parameter storing the result of this function.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_DIM_ZERO
- * 	If \p deg == 0.
- * @return TS_DEG_GE_NCTRLP
- * 	If \p deg >= \p n_ctrlp.
- * @return TS_NUM_KNOTS
- * 	If \p type == ::TS_BEZIERS and (\p n_ctrlp % \p deg + 1) != 0.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_bspline_new(size_t n_ctrlp, size_t dim, size_t deg,
-	tsBSplineType type, tsBSpline *_spline_);
-
-/**
- * The copy constructor of tsBSpline.
- *
- * Creates a deep copy of \p original and stores the result in \p \_copy\_.
- *
- * On error, all values of \p \_copy\_ are set to 0/NULL. Does nothing, if
- * \p original == \p \_copy\_.
- *
- * @param original
- * 	The spline to deep copy.
- * @param \_copy\_
- * 	The output parameter storing the copied values of \p original.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_bspline_copy(const tsBSpline *original, tsBSpline *_copy_);
-
-/**
- * The move constructor of tsBSpline.
- *
- * Moves all values from \p from to \p \_to\_ and calls ::ts_bspline_default
- * on \p from afterwards. Does nothing, if \p from == \p \_to\_.
- * 
- * @param from
- * 	The spline whose values are moved to \p \_to\_.
- * @param \_to\_
- * 	The output parameter storing the moved values of \p from.
- */
-void ts_bspline_move(tsBSpline *from, tsBSpline *_to_);
-
-/**
- * The destructor of tsBSpline.
- *
- * Frees all dynamically allocated memory in \p \_spline\_ and calls
- * ::ts_bspline_default afterwards.
- * 
- * @param \_spline\_
- * 	The spline to free.
- */
-void ts_bspline_free(tsBSpline *_spline_);
-
-/* ------------------------------------------------------------------------- */
-
-/**
- * The default constructor of tsDeBoorNet.
- *
- * All values of \p \_deBoorNet\_ are set to 0/NULL.
- * 
- * @param \_deBoorNet\_
- * 	The net whose values are set 0/NULL.
- */
-void ts_deboornet_default(tsDeBoorNet *_deBoorNet_);
-
-/**
- * The copy constructor of tsDeBoorNet.
- *
- * Creates a deep copy of \p original and stores the result in \p \_copy\_.
- *
- * On error, all values of \p _copy_ are set to 0/NULL. Does nothing, if
- * \p original == \p \_copy\_.
- *
- * @param original
- * 	The net to deep copy.
- * @param \_copy\_
- * 	The output parameter storing the copied values of \p original.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_deboornet_copy(const tsDeBoorNet *original, tsDeBoorNet *_copy_);
-
-/**
- * The move constructor of tsDeBoorNet.
- *
- * Moves all values from \p from to \p \_to\_ and calls ::ts_deboornet_default
- * on \p from afterwards. Does nothing, if \p from == \p \_to\_.
- * 
- * @param from
- * 	The net whose values are moved to \p \_to\_.
- * @param \_to\_
- * 	The output parameter storing the moved values of \p from.
- */
-void ts_deboornet_move(tsDeBoorNet *from, tsDeBoorNet *_to_);
-
-/**
- * The destructor of tsDeBoorNet.
- *
- * Frees all dynamically allocated memory in \p \_deBoorNet\_ and calls
- * ::ts_deboornet_default afterwards.
- * 
- * @param \_deBoorNet\_
- * 	The net to free.
- */
-void ts_deboornet_free(tsDeBoorNet *_deBoorNet_);
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Interpolation and Approximation Functions                                *
-*                                                                             *
-* The following section contains functions to interpolate and approximate     *
-* arbitrary splines.                                                          *
-*                                                                             *
-******************************************************************************/
-/**
- * Performs a cubic spline interpolation using the thomas algorithm, see:
- * 
- *     https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm
- *     http://www.math.ucla.edu/~baker/149.1.02w/handouts/dd_splines.pdf
- *     http://www.bakoma-tex.com/doc/generic/pst-bspline/pst-bspline-doc.pdf
- *
- * The resulting spline is a sequence of bezier curves connecting each point
- * in \p points. Each bezier curve _b_ is of degree 3 with \p dim being the
- * dimension of the each control point in _b_. The total number of control
- * points is (\p n - 1) * 4.
- *
- * On error, all values of \p \_spline\_ are set to 0/NULL.
- *
- * Note: \p n is the number of points in \p points and not the length of
- * \p points. For instance, the follwing point vector yields to \p n = 4 and
- * \p dim = 2:
- * 
- *     [x0, y0, x1, y1, x2, y2, x3, y3]
- *
- * @param points
- * 	The points to interpolate.
- * @param n
- * 	The number of points in \p points.
- * @param dim
- * 	The dimension of each control point in \p \_spline\_.
- * @param \_spline\_
- * 	The output parameter storing the result of this function.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_DIM_ZERO
- * 	If \p dim == 0.
- * @return TS_DEG_GE_NCTRLP
- * 	If \p n < 2.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_bspline_interpolate_cubic(const tsReal *points, size_t n,
-	size_t dim, tsBSpline *_spline_);
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Query Functions                                                          *
-*                                                                             *
-* The following section contains functions to query splines.                  *
-*                                                                             *
-******************************************************************************/
-/**
- * Evaluates \p spline at knot value \p u and stores the result in
- * \p \_deBoorNet\_.
- *
- * On error, all values of \p \_deBoorNet\_ are set to 0/NULL.
- *
- * @param spline
- * 	The spline to evaluate.
- * @param u
- * 	The knot value to evaluate.
- * @param \_deBoorNet\_
- * 	The output parameter storing the evaluation result.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MULTIPLICITY
- * 	If multiplicity s(\p u) > order of \p spline.
- * @return TS_U_UNDEFINED
- * 	If \p spline is not defined at knot value \p u.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_bspline_eval(const tsBSpline *spline, tsReal u,
-	tsDeBoorNet *_deBoorNet_);
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Transformation functions                                                 *
-*                                                                             *
-* TinySpline is a library focusing on transformations. That is, most          *
-* functions are used to transform splines by modifying their state, e.g.,     *
-* their number of control points, their degree, and so on. Accordingly, each  *
-* transformation functions specifies an input and output parameter (along     *
-* with the other parameters required to calculate the actual transformation). *
-* By passing a different pointer to the output parameter, the transformation  *
-* result is calculated and stored without changing the state of the input.    *
-* This is in particular useful when dealing with errors as the original state *
-* will never be modified. For instance, let's have a look at the following    *
-* code snippet:                                                               *
-*                                                                             *
-*     tsBSpline in = ...    // an arbitrary spline                            *
-*     tsBSpline out;        // result of transformation                       *
-*                                                                             *
-*     // Subdivide 'in' into sequence of bezier curves and store the result   *
-*     // in 'out'. Does not change 'in' in any way.                           *
-*     tsError err = ts_bspline_to_beziers(&in, &out);                         *
-*     if (err != TS_SUCCESS) {                                                *
-*         // fortunately, 'in' has not been changed                           *
-*     }                                                                       *
-*                                                                             *
-* Even if 'ts_bspline_to_beziers' fails, the state of 'in' has not been       *
-* changed allowing you to handle the error properly.                          *
-*                                                                             *
-* Unless stated otherwise, the order of the parameters for transformation     *
-* functions is:                                                               *
-*                                                                             *
-*     function(input, [additional_input], output, [additional_output])        *
-*                                                                             *
-* 'additional_input' are parameters required to calculate the actual          *
-* transformation. 'additional_output' are parameters storing further result.  *
-*                                                                             *
-* Note: None of TinySpline's transformation functions frees the memory of the *
-*       output parameter. Thus, when using the same output parameter multiple *
-*       times, make sure to free memory before each call. Otherwise, you will *
-*       have a bad time with memory leaks:                                    *
-*                                                                             *
-*     tsBSpline in = ...                  // an arbitrary spline              *
-*     tsBSpline out;                      // result of transformations        *
-*                                                                             *
-*     ts_bspline_to_beziers(&in, &out);   // first transformation             *
-*     ...                                 // some code                        *
-*     ts_bspline_free(&out);              // avoid memory leak.               *
-*     ts_bspline_buckle(&in, &out);       // next transformation              *
-*                                                                             *
-* If you want to modify your input directly without having a separate output, *
-* pass it as input and output at once:                                        *
-*                                                                             *
-*     tsBSpline s = ...                       // an arbitrary spline          *
-*     tsReal *knots = ...                     // a knot vector                *
-*                                                                             *
-*     ts_bspline_set_knots(&s, knots, &s);    // copy 'knots' into 's'        *
-*                                                                             *
-* Note: If a transformation function fails *and* input != output, all fields  *
-*       of the output parameter are set to 0/NULL. If input == output, your   *
-*       input may have an invalid state in case of errors.                    *
-*                                                                             *
-******************************************************************************/
-/**
- * Computes the derivative of \p spline, see:
- * 
- *     http://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-derv.html
- *
- * The derivative of a spline _s_ of degree _d_ with _m_ control points and
- * _n_ knots is another spline _s'_ of degree _d-1_ with _m-1_ control points
- * and _n-2_ knots, defined over _s_ as:
- * 
- * \f{eqnarray*}{
- *   s'(u) &=& \sum_{i=0}^{n-1} N_{i+1,p-1}(u) *
- * 		(P_{i+1} - P_{i}) * p / (u_{i+p+1}-u_{i+1}) \\
- *         &=& \sum_{i=1}^{n} N_{i,p-1}(u) *
- * 		(P_{i} - P_{i-1}) * p / (u_{i+p}-u_{i})
- * \f}
- * 
- * If _s_ has a clamped knot vector, it can be shown that:
- * 
- * \f{eqnarray*}{
- *   s'(u) &=& \sum_{i=0}^{n-1} N_{i,p-1}(u) *
- * 		(P_{i+1} - P_{i}) * p / (u_{i+p+1}-u_{i+1})
- * \f}
- * 
- * where the multiplicity of the first and the last knot value _u_ is _p_
- * rather than _p+1_.
- *
- * On error, (and if \p spline != \p \_derivative\_) all values of
- * \p \_derivative\_ are set to 0/NULL.
- *
- * @param spline
- * 	The spline to derive.
- * @param \_derivative\_
- *	The output parameter storing the derivative of \p spline.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_UNDERIVABLE
- * 	If \p spline->deg < 1, \p spline->n_ctrlp < 2, or the multiplicity of
- * 	an internal knot of \p spline is greater than the degree of \p spline.
- * 	NOTE: This will be fixed in the future.
- * @return TS_MALLOC
- * 	If allocating memory failed.
- */
-tsError ts_bspline_derive(const tsBSpline *spline, tsBSpline *_derivative_);
-
-/**
- * Fills the knot vector of \p spline according to \p type with minimum knot
- * value \p min to maximum knot value \p max and stores the result in
- * \p \_result\_. Creates a deep copy of \p spline, if
- * \p spline != \p \_result\_.
- *
- * On error, (and if \p spline != \p \_result\_) all values of \p \_result\_
- * are set to 0/NULL.
- *
- * @param spline
- * 	The spline to deep copy (if \p spline != \p \_result\_) and whose knot
- * 	vector is filled according to \p type with minimum knot value \p min
- * 	and maximum knot value \p max.
- * @param type
- * 	How to fill the knot vector of \p \_result\_.
- * @param min
- * 	The minimum knot value of the knot vector of \p \_result\_.
- * @param max
- * 	The maximum knot value of the knot vector of \p \_result\_.
- * @param \_result\_
- * 	The output parameter storing the result of this function.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_DEG_GE_NCTRLP
- * 	If \p spline->n_knots < 2*(\p original->deg+1). We can reuse this
- * 	error code because \p spline->n_knots < 2*(\p spline->deg+1) implies
- * 	\p spline->deg >= \p spline->n_ctrlp. Furthermore, using
- * 	TS_DEG_GE_NCTRLP instead of TS_NUM_KNOTS ensures that TS_NUM_KNOTS is
- * 	not used twice for this function. To be more fail-safe,
- * 	\p spline->deg+1 instead of \p spline->order is used, to make sure
- * 	that \p spline->deg+1 >= 1.
- * @return TS_NUM_KNOTS
- * 	If \p type == TS_BEZIERS and
- * 	\p spline->n_knots % \p spline->order != 0.
- * @return TS_KNOTS_DECR
- * 	If \p min >= \p max. (::ts_fequals is used to determine whether
- * 	\p min == \p max).
- * @return TS_MALLOC
- * 	If \p spline != \p \_result\_ and allocating memory failed.
- */
-tsError ts_bspline_fill_knots(const tsBSpline *spline, tsBSplineType type,
-	tsReal min, tsReal max, tsBSpline *_result_);
-
-/**
- * Inserts the knot value \p u \p n times into \p spline and stores the result
- * in \p \_result\_. Creates a deep copy of \p spline, if
- * \p spline != \p \_result\_.
- * 
- * On error, (and if \p spline != \p \_result\_) all values of \p \_result\_
- * are set to 0/NULL.
- * 
- * @param spline
- * 	The spline to deep copy (if \p spline != \p \_result\_) and whose knot
- * 	vector is extended with \p u \p n times.
- * @param u
- * 	The knot value to insert.
- * @param n
- * 	How many times \p u should be inserted.
- * @param \_result\_
- * 	The output parameter storing the updated knot vector.
- * @param \_k\_
- * 	The output parameter storing the last index of \p u in \p \_result\_.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If \p spline != \p \_result\_ and allocating memory failed.
- */
-tsError ts_bspline_insert_knot(const tsBSpline *spline, tsReal u, size_t n,
-	tsBSpline *_result_, size_t *_k_);
-
-/**
- * Resizes \p spline by \p n (number of control points) and stores the result
- * in \p \_resized\_. Creates a deep copy of \p spline, if
- * \p spline != \p \_result\_. If \p back != 0 \p spline is resized at the
- * end. If \p back == 0 \p spline is resized at front.
- *
- * On error, (and if \p spline != \p \_result\_) all values of \p \_result\_
- * are set to 0/NULL.
- *
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_DEG_GE_NCTRLP
- * 	If the degree of \p \_resized\_ would be >= the number of the control
- * 	points of \p \_resized\_.
- * @return TS_DIM_ZERO
- * 	If \p spline != \p \_result\_ and \p spline->dim == 0.
- * @return TS_DEG_GE_NCTRLP
- * 	If \p spline != \p \_result\_ and
- * 	\p spline->deg >= \p spline->n_ctrlp.
- * @return TS_MALLOC
- * 	If \p spline != \p \_result\_ and allocating memory failed.
- */
-tsError ts_bspline_resize(const tsBSpline *spline, int n, int back,
-	tsBSpline *_resized_);
-
-/**
- * Splits \p spline at \p u and stores the result in \p \_split\_. That is,
- * \p u is inserted _n_ times such that s(\p u) == \p \_split\_->order.
- * Creates a deep copy of \p spline, if \p spline != \p \_split\_.
- * 
- * On error, (and if \p spline != \p \_split\_) all values of \p \_split\_
- * are set to 0/NULL.
- * 
- * @param spline
- * 	The spline to deep copy (if \p spline != \p \_result\_) and split.
- * @param u
- * 	The split point.
- * @param \_split\_
- * 	The output parameter storing the split spline.
- * @param \_k\_
- * 	The output parameter storing the last index of \p u in \p \_split\_.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If \p spline != \p \_split\_ and allocating memory failed.
- */
-tsError ts_bspline_split(const tsBSpline *spline, tsReal u, tsBSpline *_split_,
-	size_t *_k_);
-
-/**
- * Buckles \p spline by \p b and stores the result in \p \_buckled\_. Creates
- * a deep copy of \p spline, if \p spline != \p \_buckled\_.
- *
- * This function is based on:
- * 
- *      Holten, Danny. "Hierarchical edge bundles: Visualization of adjacency
- *      relations in hierarchical data." Visualization and Computer Graphics,
- *      IEEE Transactions on 12.5 (2006): 741-748.
- * 
- * Holten calls it "straightening" (page 744, equation 1).
- *
- * Usually, the range of \p b is: 0.0 <= \p b <= 1.0 with 0 yielding to a line
- * connecting the first and the last control point (no buckle) and 1 keeping
- * the original shape (maximum buckle). If \b < 0 or \b > 1 the behaviour is
- * undefined, though, it will not result in an error.
- *
- * On error, (and if \p spline != \p \_buckled\_) all values of \p \_buckled\_
- * are set to 0/NULL.
- *
- * @param spline
- * 	The spline to buckle by \p b.
- * @param b
- * 	The buckle factor (usually 0.0 <= \p b <= 1.0).
- * @param \_buckled\_
- * 	The output parameter storing the buckled spline.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If \p spline != \p \_buckled\_ and allocating memory failed.
- */
-tsError ts_bspline_buckle(const tsBSpline *spline, tsReal b,
-	tsBSpline *_buckled_);
-
-/**
- * Subdivides \p spline into a sequence of Bezier curvs by splitting it at
- * each internal knot value. Creates a deep copy of \p spline, if
- * \p spline != \p \_beziers\_.
- * 
- * On error, (and if \p spline != \p \_beziers\_) all values of \p \_beziers\_
- * are set to 0/NULL.
- * 
- * @param spline
- * 	The spline to subdivide into a sequence of Bezier curves.
- * @param \_beziers\_
- * 	The output parameter storing the sequence of Bezier curves.
- * @return TS_SUCCESS
- * 	On success.
- * @return TS_MALLOC
- * 	If \p spline != \p \_beizers\_ and allocating memory failed.
- */
-tsError ts_bspline_to_beziers(const tsBSpline *spline, tsBSpline *_beziers_);
-
-
-
-/******************************************************************************
-*                                                                             *
-* :: Utility Functions                                                        *
-*                                                                             *
-* The following section contains utility functions used by TinySpline which   *
-* also may be helpful when using this library.                                *
-*                                                                             *
-******************************************************************************/
-/**
- * Compares the tsReal values \p x and \p y using an absolute and relative
- * epsilon environment.
- *
- * @param x
- * 	The x value to compare.
- * @param y
- * 	The y value to compare.
- * @return 1
- * 	If \p x is equals to \p y.
- * @return 0
- * 	Otherwise.
- */
-int ts_fequals(tsReal x, tsReal y);
-
-/**
- * Returns the error message associated to \p err. Returns "unknown error" if
- * \p err is no associated (indicating a bug) or is TS_SUCCESS (which is not
- * an actual error).
- */
-const char* ts_enum_str(tsError err);
-
-/**
- * Returns the error code associated to \p str or TS_SUCCESS if \p str is not
- * associated. Keep in mind that by concept "unknown error" is not associated,
- * though, TS_SUCCESS is returned.
- */
-tsError ts_str_enum(const char *str);
-
-/**
- * Fills the given array \p arr with \p val from \p arr+0 to \p arr+ \p num
- * (exclusive).
- */
-void ts_arr_fill(tsReal *arr, size_t num, tsReal val);
-
-/**
- * Returns the euclidean distance of \p x and \p y consisting of \p dim
- * components, respectively.
- *
- * @param x
- * 	The x value.
- * @param y
- * 	The y value.
- * @param dim
- * 	The dimension of \p x and \p y.
- * @return
- * 	The euclidean distanc of \p x and \p y.
- */
-tsReal ts_ctrlp_dist2(const tsReal *x, const tsReal *y, size_t dim);
-
-
-
-#ifdef	__cplusplus
-}
-#endif
-
-#endif	/* TINYSPLINE_H */
-

src/decition/decition_brain/decision/tinyspline/tinyspline.pri

@@ -1,7 +0,0 @@
-HEADERS += \
-    $$PWD/tinyspline.h \
-    $$PWD/tinysplinecpp.h
-
-SOURCES += \
-    $$PWD/tinyspline.c \
-    $$PWD/tinysplinecpp.cpp

src/decition/decition_brain/decision/tinyspline/tinysplinecpp.cpp

@@ -1,350 +0,0 @@
-#include <tinyspline_ros/tinysplinecpp.h>
-#include <stdexcept>
-#include <cstdio>
-
-// Surpress warning C4996 (sprintf_s).
-#ifdef _MSC_VER
-#pragma warning(disable:4996)
-#endif
-
-/******************************************************************************
-*                                                                             *
-* DeBoorNet                                                                   *
-*                                                                             *
-******************************************************************************/
-tinyspline::DeBoorNet::DeBoorNet()
-{
-	ts_deboornet_default(&net);
-}
-
-tinyspline::DeBoorNet::DeBoorNet(const tinyspline::DeBoorNet &other)
-{
-	tsError err = ts_deboornet_copy(&other.net, &net);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-}
-
-tinyspline::DeBoorNet::~DeBoorNet()
-{
-	ts_deboornet_free(&net);
-}
-
-tinyspline::DeBoorNet & tinyspline::DeBoorNet::operator=(
-	const tinyspline::DeBoorNet &other)
-{
-	if (&other != this) {
-		tsError err = ts_deboornet_copy(&other.net, &net);
-		if (err < 0)
-			throw std::runtime_error(ts_enum_str(err));
-	}
-	return *this;
-}
-
-tinyspline::real tinyspline::DeBoorNet::knot() const
-{
-	return ts_deboornet_knot(&net);
-}
-
-size_t tinyspline::DeBoorNet::index() const
-{
-	return ts_deboornet_index(&net);
-}
-
-size_t tinyspline::DeBoorNet::multiplicity() const
-{
-	return ts_deboornet_multiplicity(&net);
-}
-
-size_t tinyspline::DeBoorNet::numInsertions() const
-{
-	return ts_deboornet_num_insertions(&net);
-}
-
-size_t tinyspline::DeBoorNet::dimension() const
-{
-	return ts_deboornet_dimension(&net);
-}
-
-std::vector<tinyspline::real> tinyspline::DeBoorNet::points() const
-{
-	tsReal *points;
-	tsError err = ts_deboornet_points(&net, &points);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	size_t num_points = ts_deboornet_num_points(&net);
-	tinyspline::real *begin = points;
-	tinyspline::real *end = begin + num_points * dimension();
-	std::vector<tinyspline::real> vec =
-		std::vector<tinyspline::real>(begin, end);
-	delete points;
-	return vec;
-}
-
-std::vector<tinyspline::real> tinyspline::DeBoorNet::result() const
-{
-	tsReal *result;
-	tsError err = ts_deboornet_result(&net, &result);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	size_t num_result = ts_deboornet_num_result(&net);
-	tinyspline::real *begin = result;
-	tinyspline::real *end = begin + num_result * dimension();
-	std::vector<tinyspline::real> vec =
-		std::vector<tinyspline::real>(begin, end);
-	delete result;
-	return vec;
-}
-
-tsDeBoorNet * tinyspline::DeBoorNet::data()
-{
-	return &net;
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* BSpline                                                                     *
-*                                                                             *
-******************************************************************************/
-tinyspline::BSpline::BSpline()
-{
-	ts_bspline_default(&spline);
-}
-
-tinyspline::BSpline::BSpline(const tinyspline::BSpline &other)
-{
-	tsError err = ts_bspline_copy(&other.spline, &spline);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-}
-
-tinyspline::BSpline::BSpline(size_t nCtrlp, size_t dim, size_t deg,
-	tinyspline::BSpline::type type)
-{
-	tsError err = ts_bspline_new(nCtrlp, dim, deg, type, &spline);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-}
-
-tinyspline::BSpline::~BSpline()
-{
-	ts_bspline_free(&spline);
-}
-
-tinyspline::BSpline & tinyspline::BSpline::operator=(
-	const tinyspline::BSpline &other)
-{
-	if (&other != this) {
-		tsError err = ts_bspline_copy(&other.spline, &spline);
-		if (err < 0)
-			throw std::runtime_error(ts_enum_str(err));
-	}
-	return *this;
-}
-
-tinyspline::DeBoorNet tinyspline::BSpline::operator()(tinyspline::real u) const
-{
-	return eval(u);
-}
-
-size_t tinyspline::BSpline::degree() const
-{
-	return ts_bspline_degree(&spline);
-}
-
-size_t tinyspline::BSpline::order() const
-{
-	return ts_bspline_order(&spline);
-}
-
-size_t tinyspline::BSpline::dimension() const
-{
-	return ts_bspline_dimension(&spline);
-}
-
-std::vector<tinyspline::real> tinyspline::BSpline::controlPoints() const
-{
-	tsReal *ctrlp;
-	tsError err = ts_bspline_control_points(&spline, &ctrlp);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	size_t num_ctrlp = ts_bspline_num_control_points(&spline);
-	tinyspline::real *begin  = ctrlp;
-	tinyspline::real *end = begin + num_ctrlp * dimension();
-	std::vector<tinyspline::real> vec =
-		std::vector<tinyspline::real>(begin, end);
-	delete ctrlp;
-	return vec;
-}
-
-std::vector<tinyspline::real> tinyspline::BSpline::knots() const
-{
-	tsReal *knots;
-	tsError err = ts_bspline_knots(&spline, &knots);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	size_t num_knots = ts_bspline_num_knots(&spline);
-	tinyspline::real *begin = knots;
-	tinyspline::real *end = begin + num_knots;
-	std::vector<tinyspline::real> vec =
-		std::vector<tinyspline::real>(begin, end);
-	delete knots;
-	return vec;
-}
-
-tsBSpline * tinyspline::BSpline::data()
-{
-	return &spline;
-}
-
-tinyspline::DeBoorNet tinyspline::BSpline::eval(tinyspline::real u) const
-{
-	tinyspline::DeBoorNet deBoorNet;
-	tsError err = ts_bspline_eval(&spline, u, deBoorNet.data());
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return deBoorNet;
-}
-
-void tinyspline::BSpline::setControlPoints(
-	const std::vector<tinyspline::real> &ctrlp)
-{
-	size_t expected = ts_bspline_len_control_points(&spline);
-	size_t actual = ctrlp.size();
-	if (expected != actual) {
-		char expected_str[32];
-		char actual_str[32];
-		sprintf(expected_str, "%zu", expected);
-		sprintf(actual_str, "%zu", actual);
-		throw std::runtime_error(
-			"Expected size: " + std::string(expected_str) +
-			", Actual size: " + std::string(actual_str));
-	}
-	tsError err = ts_bspline_set_control_points(&spline, ctrlp.data());
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-}
-
-void tinyspline::BSpline::setKnots(const std::vector<tinyspline::real> &knots)
-{
-	size_t expected = ts_bspline_num_knots(&spline);
-	size_t actual = knots.size();
-	if (expected != actual) {
-		char expected_str[32];
-		char actual_str[32];
-		sprintf(expected_str, "%zu", expected);
-		sprintf(actual_str, "%zu", actual);
-		throw std::runtime_error(
-			"Expected size: " + std::string(expected_str) +
-			", Actual size: " + std::string(actual_str));
-	}
-	tsError err = ts_bspline_set_knots(&spline, knots.data());
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-}
-
-tinyspline::BSpline tinyspline::BSpline::fillKnots(tsBSplineType type,
-	tinyspline::real min, tinyspline::real max) const
-{
-	tinyspline::BSpline bs;
-	tsError err = ts_bspline_fill_knots(
-		&spline, type, min, max, &bs.spline);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bs;
-}
-
-tinyspline::BSpline tinyspline::BSpline::insertKnot(tinyspline::real u,
-	size_t n) const
-{
-	tinyspline::BSpline bs;
-	size_t k;
-	tsError err = ts_bspline_insert_knot(&spline, u, n, &bs.spline, &k);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bs;
-}
-
-tinyspline::BSpline tinyspline::BSpline::resize(int n, int back) const
-{
-	tinyspline::BSpline bs;
-	tsError err = ts_bspline_resize(&spline, n, back, &bs.spline);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bs;
-}
-
-tinyspline::BSpline tinyspline::BSpline::split(tinyspline::real u) const
-{
-	tinyspline::BSpline bs;
-	size_t k;
-	tsError err = ts_bspline_split(&spline, u, &bs.spline, &k);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bs;
-}
-
-tinyspline::BSpline tinyspline::BSpline::buckle(tinyspline::real b) const
-{
-	tinyspline::BSpline bs;
-	tsError err = ts_bspline_buckle(&spline, b, &bs.spline);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bs;
-}
-
-tinyspline::BSpline tinyspline::BSpline::toBeziers() const
-{
-	tinyspline::BSpline bs;
-	tsError err = ts_bspline_to_beziers(&spline, &bs.spline);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bs;
-}
-
-tinyspline::BSpline tinyspline::BSpline::derive() const
-{
-	tinyspline::BSpline bs;
-	tsError err = ts_bspline_derive(&spline, &bs.spline);
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bs;
-}
-
-
-
-/******************************************************************************
-*                                                                             *
-* Utils                                                                       *
-*                                                                             *
-******************************************************************************/
-tinyspline::BSpline tinyspline::Utils::interpolateCubic(
-	const std::vector<tinyspline::real> *points, size_t dim)
-{
-	if (dim == 0)
-		throw std::runtime_error(ts_enum_str(TS_DIM_ZERO));
-	if (points->size() % dim != 0)
-		throw std::runtime_error("#points % dim == 0 failed");
-	tinyspline::BSpline bspline;
-	tsError err = ts_bspline_interpolate_cubic(
-		points->data(), points->size()/dim, dim, bspline.data());
-	if (err < 0)
-		throw std::runtime_error(ts_enum_str(err));
-	return bspline;
-}
-
-bool tinyspline::Utils::fequals(tinyspline::real x, tinyspline::real y)
-{
-	return ts_fequals(x, y) == 1;
-}
-
-std::string tinyspline::Utils::enum_str(tsError err)
-{
-	return std::string(ts_enum_str(err));
-}
-
-tsError tinyspline::Utils::str_enum(std::string str)
-{
-	return ts_str_enum(str.c_str());
-}

src/decition/decition_brain/decision/tinyspline/tinysplinecpp.h

@@ -1,90 +0,0 @@
-#pragma once
-
-#include <tinyspline_ros/tinyspline.h>
-#include <vector>
-#include <string>
-
-namespace tinyspline {
-
-typedef tsReal real;
-
-class DeBoorNet {
-public:
-	/* Constructors & Destructors */
-	DeBoorNet();
-	DeBoorNet(const DeBoorNet &other);
-	~DeBoorNet();
-
-	/* Operators */
-	DeBoorNet& operator=(const DeBoorNet &other);
-
-	/* Getter */
-	real knot() const;
-	size_t index() const;
-	size_t multiplicity() const;
-	size_t numInsertions() const;
-	size_t dimension() const;
-	std::vector<real> points() const;
-	std::vector<real> result() const;
-	tsDeBoorNet * data();
-
-private:
-	tsDeBoorNet net;
-};
-
-class BSpline {
-public:
-	typedef tsBSplineType type;
-
-	/* Constructors & Destructors */
-	BSpline();
-	BSpline(const BSpline &other);
-	explicit BSpline(size_t nCtrlp, size_t dim = 2, size_t deg = 3,
-		tinyspline::BSpline::type type = TS_CLAMPED);
-	~BSpline();
-
-	/* Operators */
-	BSpline & operator=(const BSpline &other);
-	DeBoorNet operator()(real u) const;
-
-	/* Getter */
-	size_t degree() const;
-	size_t order() const;
-	size_t dimension() const;
-	std::vector<real> controlPoints() const;
-	std::vector<real> knots() const;
-	tsBSpline * data();
-
-	/* Query */
-	DeBoorNet eval(real u) const;
-
-	/* Modifications */
-	void setControlPoints(const std::vector<real> &ctrlp);
-	void setKnots(const std::vector<real> &knots);
-
-	/* Transformations */
-	BSpline fillKnots(tsBSplineType type, real min, real max) const;
-	BSpline insertKnot(real u, size_t n) const;
-	BSpline resize(int n, int back) const;
-	BSpline split(real u) const;
-	BSpline buckle(real b) const;
-	BSpline toBeziers() const;
-	BSpline derive() const;
-
-private:
-	tsBSpline spline;
-};
-
-class Utils {
-public:
-	static BSpline interpolateCubic(
-		const std::vector<real> *points, size_t dim);
-	static bool fequals(real x, real y);
-	static std::string enum_str(tsError err);
-	static tsError str_enum(std::string str);
-
-private:
-	Utils() {}
-};
-
-}

src/decition/decition_brain/decision/transfer.cpp

@@ -1,138 +0,0 @@
-#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/decision/transfer.h

@@ -1,27 +0,0 @@
-#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/decition/adc_adapter/base_adapter.cpp

@@ -1,9 +1,9 @@
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
 
 
 

src/decition/decition_brain/decition/adc_adapter/base_adapter.h

@@ -4,10 +4,10 @@
 
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
 
 namespace iv {
 namespace decition {

src/decition/decition_brain/decition/adc_adapter/bus_adapter.cpp

@@ -1,4 +1,4 @@
-#include <decision/adc_adapter/bus_adapter.h>
+#include <decition/adc_adapter/bus_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_adapter/bus_adapter.h

@@ -3,13 +3,13 @@
 
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
 #include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.h>
+#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 {

src/decition/decition_brain/decition/adc_adapter/ge3_adapter.cpp

@@ -1,4 +1,4 @@
-#include <decision/adc_adapter/ge3_adapter.h>
+#include <decition/adc_adapter/ge3_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_adapter/ge3_adapter.h

@@ -4,13 +4,13 @@
 
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.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 {

src/decition/decition_brain/decition/adc_adapter/hapo_adapter.cpp

@@ -1,4 +1,4 @@
-#include <decision/adc_adapter/hapo_adapter.h>
+#include <decition/adc_adapter/hapo_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_adapter/hapo_adapter.h

@@ -3,13 +3,13 @@
 
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
 #include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.h>
+#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 {

src/decition/decition_brain/decition/adc_adapter/qingyuan_adapter.cpp

@@ -1,4 +1,4 @@
-#include <decision/adc_adapter/qingyuan_adapter.h>
+#include <decition/adc_adapter/qingyuan_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_adapter/qingyuan_adapter.h

@@ -2,13 +2,13 @@
 #define QINGYUAN_ADAPTER_H
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.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 {

src/decition/decition_brain/decition/adc_adapter/vv7_adapter.cpp

@@ -1,4 +1,4 @@
-#include <decision/adc_adapter/vv7_adapter.h>
+#include <decition/adc_adapter/vv7_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_adapter/vv7_adapter.h

@@ -1,14 +1,16 @@
 #ifndef VV7_ADAPTER_H
 #define VV7_ADAPTER_H
 
+
+
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.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 {

src/decition/decition_brain/decition/adc_adapter/yuhesen_adapter.cpp

@@ -1,4 +1,4 @@
-#include <decision/adc_adapter/yuhesen_adapter.h>
+#include <decition/adc_adapter/yuhesen_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_adapter/yuhesen_adapter.h

@@ -1,14 +1,16 @@
 #ifndef YUHESEN_ADAPTER_H
 #define YUHESEN_ADAPTER_H
 
+
+
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.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 {

src/decition/decition_brain/decition/adc_adapter/zhongche_adapter.cpp

@@ -1,5 +1,5 @@
 
-#include <decision/adc_adapter/zhongche_adapter.h>
+#include <decition/adc_adapter/zhongche_adapter.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_adapter/zhongche_adapter.h

@@ -1,14 +1,16 @@
 #ifndef ZHONGCHE_ADAPTER_H
 #define ZHONGCHE_ADAPTER_H
 
+
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_adapter/base_adapter.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/decide_gps_00.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 {

src/decition/decition_brain/decition/adc_controller/base_controller.cpp

@@ -1,9 +1,9 @@
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_controller/base_controller.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_controller/base_controller.h>
 
 
 

src/decition/decition_brain/decition/adc_controller/base_controller.h

@@ -3,10 +3,10 @@
 #define _IV_DECITION__BASE_CONTROLLER_
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
 #include<vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
 
 namespace iv {
 namespace decition {

src/decition/decition_brain/decition/adc_controller/pid_controller.cpp

@@ -1,4 +1,4 @@
-#include <decision/adc_controller/pid_controller.h>
+#include <decition/adc_controller/pid_controller.h>
 #include <common/constants.h>
 #include <common/car_status.h>
 #include <math.h>

src/decition/decition_brain/decition/adc_controller/pid_controller.h

@@ -2,12 +2,12 @@
 #define _IV_DECITION__PID_CONTROLLER_
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
 #include<vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_controller/base_controller.h>
-#include <decision/adc_tools/transfer.h>
+#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 {

src/decition/decition_brain/decition/adc_planner/base_planner.cpp

@@ -1,11 +1,11 @@
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
-#include <vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
-#include <decision/adc_tools/transfer.h>
+#include<vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_tools/transfer.h>
 #include "perception/perceptionoutput.h"
-#include <decision/adc_planner/base_planner.h>
+#include <decition/adc_planner/base_planner.h>
 
 
 

src/decition/decition_brain/decition/adc_planner/base_planner.h

@@ -2,10 +2,10 @@
 #define _IV_DECITION__BASE_PLANNER_
 
 #include <common/gps_type.h>
-#include <decision/decition_type.h>
+#include <decition/decition_type.h>
 #include <common/obstacle_type.h>
 #include<vector>
-#include <decision/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
 #include "perception/perceptionoutput.h"
 
 namespace iv {

src/decition/decition_brain/decition/adc_planner/dubins_planner.cpp

@@ -1,6 +1,6 @@
-#include <decision/adc_planner/dubins_planner.h>
-#include <decision/adc_tools/transfer.h>
-#include <decision/adc_tools/parameter_status.h>
+#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>

src/decition/decition_brain/decition/adc_planner/dubins_planner.h

@@ -1,7 +1,7 @@
 #ifndef DUBINS_PLANNER_H
 #define DUBINS_PLANNER_H
 #include "base_planner.h"
-#include <decision/adc_tools/dubins.h>
+#include <decition/adc_tools/dubins.h>
 #include <QCoreApplication>
 
 #include <math.h>