123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626 |
- #include "ivpark_simple.h"
- #include "adc_tools/compute_00.h"
- #include "adc_tools/transfer.h"
- #include <chrono>
- using namespace iv;
- ivpark_simple::ivpark_simple()
- {
- }
- bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
- {
- GPS_INS nowgps,aimgps;
- std::vector<std::vector<GPS_INS>> xvectordTPoint;
- std::vector<double> xvectorRearDis;
- std::vector<double> xvectorAngle;
- std::vector<int> xvectortype;
- std::vector<GPS_INS> xvectoraimgps;
- std::vector<iv::simpleparkspace> xvectorsimpleparkspace = GetParkSpace();
- xvectordTPoint.clear();
- xvectorRearDis.clear();
- xvectorAngle.clear();
- unsigned int i;
- unsigned int nsize = static_cast<unsigned int >(xvectorsimpleparkspace.size());
- nowgps.gps_lat = fLat;
- nowgps.gps_lng = fLon;
- nowgps.ins_heading_angle = fHeading;
- for(i=0;i<nsize;i++)
- {
- iv::simpleparkspace xpark = xvectorsimpleparkspace[i];
- iv::GPS_INS aimgps;
- aimgps.gps_lat = xpark.mfLat;
- aimgps.gps_lng = xpark.mfLon;
- aimgps.ins_heading_angle = xpark.mfHeading;
- std::vector<GPS_INS> TPoints;
- double fRearDis,fAngle;
- if(xpark.mnParkType == 1) //side park
- {
- if(iv::decition::Compute00().bocheDirectCompute(nowgps,aimgps,TPoints,fAngle,fRearDis) == 1)
- {
- xvectordTPoint.push_back(TPoints);
- xvectorAngle.push_back(fAngle);
- xvectorRearDis.push_back(fRearDis);
- xvectoraimgps.push_back(aimgps);
- xvectortype.push_back(1);
- }
- }
- if(xpark.mnParkType == 0)
- {
- if(iv::decition::Compute00().bocheCompute(nowgps,aimgps,TPoints,fAngle,fRearDis) == 1)
- {
- xvectordTPoint.push_back(TPoints);
- xvectorAngle.push_back(fAngle);
- xvectorRearDis.push_back(fRearDis);
- xvectoraimgps.push_back(aimgps);
- xvectortype.push_back(0);
- }
- }
- }
- nsize = static_cast<unsigned int >(xvectordTPoint.size());
- if(nsize<1)
- {
- return false;
- }
- unsigned int nsel = 0;
- //Select rear dis >1.0 and small
- for(i =1;i<nsize;i++)
- {
- if((xvectorRearDis[i] < xvectorRearDis[nsel])&&(xvectorRearDis[i]>1.0))
- {
- nsel = i;
- }
- else
- {
- if(xvectorRearDis[i]<1.0)
- {
- if(xvectorRearDis[nsel]>5.0)
- {
- nsel = i;
- }
- }
- else
- {
- if((xvectorRearDis[i]<3.0)&&(xvectorRearDis[nsel]<0.5))
- {
- nsel = i;
- }
- }
- }
- }
- if(xvectortype[nsel] == 0)
- {
- iv::decition::Compute00().nearTpoint = xvectordTPoint[nsel].at(0);
- iv::decition::Compute00().farTpoint = xvectordTPoint[nsel].at(1);
- iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
- iv::decition::Compute00().nParkType = 0;
- maimgps = xvectoraimgps[nsel];
- }
- if(xvectortype[nsel] == 1)
- {
- iv::decition::Compute00().dTpoint0 = xvectordTPoint[nsel].at(0);
- iv::decition::Compute00().dTpoint1 = xvectordTPoint[nsel].at(0);
- iv::decition::Compute00().dTpoint2 = xvectordTPoint[nsel].at(0);
- iv::decition::Compute00().dTpoint3 = xvectordTPoint[nsel].at(0);
- iv::decition::Compute00().bocheAngle = xvectorAngle[nsel];
- iv::decition::Compute00().nParkType = 1;
- maimgps = xvectoraimgps[nsel];
- }
- GaussProjCal(maimgps.gps_lng,maimgps.gps_lat,&maimgps.gps_x,&maimgps.gps_y);
- return true;
- }
- double ivpark_simple::GetDistance(iv::GPS_INS p1,iv::GPS_INS p2)
- {
- return sqrt(pow(p1.gps_x - p2.gps_x,2)+pow(p1.gps_y-p2.gps_y,2));
- }
- int ivpark_simple::GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode)
- {
- (void)fHeading;
- if(bbocheMode == false)
- {
- return 0; //Not in boche mode
- }
- static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- int64_t nstoptime_ms = 1000; //every stop time is 3
- if(xvehstate!=dRever && xvehstate!=dRever0 && xvehstate!=dRever1 && xvehstate!=dRever2
- && xvehstate!=dRever3 && xvehstate!=dRever4 && xvehstate!=reverseArr
- && xvehstate!=reverseCar && xvehstate!=reversing && xvehstate!=reverseCircle && xvehstate!=reverseDirect)
- {
- if(fSpeed>0.3)
- {
- fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
- fAcc = -0.5;
- fWheel = 0.0;
- nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- }
- else
- {
- fdSpeed = 0;fdSecSpeed = fdSpeed/3.6;
- fAcc = -0.5;
- fWheel = 0.0;
- nshift = 2; //rear
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- if(abs(nnow - nstoptiming_ms) >= nstoptime_ms)
- {
- if(iv::decition::Compute00().nParkType == 0)
- {
- xvehstate = reversing;
- mlastvehstate = xvehstate;
- }
- else
- {
- xvehstate = dRever;
- mlastvehstate = xvehstate;
- }
- }
- }
- return 1;
- }
- iv::GPS_INS nowgps;
- nowgps.gps_lat = fLat;
- nowgps.gps_lng = fLon;
- GaussProjCal(nowgps.gps_lng,nowgps.gps_lat,&nowgps.gps_x,&nowgps.gps_y);
- switch (xvehstate) {
- case reversing:
- reversingcarFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- case reverseCircle:
- reverseCircleFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- case reverseDirect:
- reverseDirectFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- case reverseArr:
- reverseArrFun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- case dRever0:
- dRever0Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- case dRever1:
- dRever1Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- case dRever2:
- dRever2Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- case dRever3:
- dRever3Fun(nowgps,fSpeed,fAcc,fWheel,nshift,fdSpeed,fdSecSpeed,xvehstate);
- break;
- default:
- return 0;
- }
- return 0;
- }
- void ivpark_simple::reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- mlastvehstate = xvehstate;
- double fdistonear = sqrt(pow(nowgps.gps_x - iv::decition::Compute00().nearTpoint.gps_x,2)+pow(nowgps.gps_y - iv::decition::Compute00().nearTpoint.gps_y,2));
- Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
- Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().nearTpoint.gps_x,iv::decition::Compute00().nearTpoint.gps_y, maimgps);
- fdistonear = fabs(pt.x - ptnear.x);
- nshift = 2;
- if(fdistonear>1.0)
- {
- fAcc = 0.0; //acc calcutale by pid
- fWheel = 0.0;
- fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- if((fSpeed>0.3)&&(fdistonear>0.3))
- {
- fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
- }
- else
- {
- fAcc = -0.5;
- xvehstate = reverseCircle;
- }
- }
- return;
- }
- void ivpark_simple::reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- (void)fSpeed;
- static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- int64_t nstoptime_ms = 3000; //every stop time is 3
- if(mlastvehstate != xvehstate)
- {
- nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- }
- mlastvehstate = xvehstate;
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- nshift = 2;
- if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
- {
- fdSpeed = 0;
- fdSecSpeed = 0;
- fAcc = -0.5;
- fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.05);
- mCircleWheel = fWheel;
- return;
- }
- fWheel = mCircleWheel;
- Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
- double angdis =fabs(nowgps.ins_heading_angle - maimgps.ins_heading_angle);
- if((fabs(pt.x)<2.0)&&(((angdis<5)||(angdis>355))))
- {
- xvehstate = reverseDirect;
- fAcc = -0.5;
- fdSpeed = 0.0;
- fWheel = 0.0;
- fdSecSpeed = 0.0;
- }
- else
- {
- fAcc = 0.0;
- fdSpeed = 2;
- fdSecSpeed = fdSecSpeed / 3.6;
- }
- return;
- }
- void ivpark_simple::reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- (void)fSpeed;
- static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- int64_t nstoptime_ms = 3000; //every stop time is 3
- if(mlastvehstate != xvehstate)
- {
- nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- }
- mlastvehstate = xvehstate;
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- nshift = 2;
- if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
- {
- fdSpeed = 0;
- fdSecSpeed = 0;
- fAcc = -0.5;
- fWheel = 0.0;
- return;
- }
- Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
- if(pt.y<0.5)
- {
- xvehstate = reverseArr;
- fAcc = -0.5;
- fdSpeed = 0.0;
- fWheel = 0.0;
- fdSecSpeed = 0.0;
- }
- else
- {
- fAcc = 0.0;
- fdSpeed = 2;
- fdSecSpeed = fdSecSpeed / 3.6;
- fWheel = 0.0;
- }
- return;
- }
- void ivpark_simple::reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- (void)nowgps;
- (void)fSpeed;
- static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- int64_t nstoptime_ms = 3000; //every stop time is 3
- if(mlastvehstate != xvehstate)
- {
- nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- }
- mlastvehstate = xvehstate;
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- nshift = 2;
- if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
- {
- fdSpeed = 0;
- fdSecSpeed = 0;
- fAcc = -0.5;
- fWheel = 0.0;
- return;
- }
- nshift = 1; //P shift
- fdSpeed = 0;
- fdSecSpeed = 0;
- fAcc = -0.5;
- fWheel = 0.0;
- xvehstate = normalRun;
- }
- void ivpark_simple::dRever0Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- mlastvehstate = xvehstate;
- Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
- Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);
- double fdistonear = fabs(pt.y - ptnear.y);
- nshift = 2;
- if(fdistonear>1.0)
- {
- fAcc = 0.0; //acc calcutale by pid
- fWheel = 0.0;
- fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- if((fSpeed>0.3)&&(fdistonear>0.3))
- {
- fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- fAcc = -0.5;
- xvehstate = dRever2;
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- }
- }
- return;
- }
- void ivpark_simple::dRever1Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- (void)fSpeed;
- static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- int64_t nstoptime_ms = 3000; //every stop time is 3
- if(mlastvehstate != xvehstate)
- {
- nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- }
- mlastvehstate = xvehstate;
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- nshift = 2;
- if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
- {
- fdSpeed = 0;
- fdSecSpeed = 0;
- fAcc = -0.5;
- fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.0);
- mCircleWheel = fWheel;
- return;
- }
- Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
- Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);
- double fdis = fabs(pt1.x - pt2.x);
- if(fdis>1.0)
- {
- fAcc = 0.0; //acc calcutale by pid
- fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- if((fSpeed>0.3)&&(fdis>0.3))
- {
- fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdis);
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- fAcc = -0.5;
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- xvehstate = dRever2;
- }
- }
- fWheel = mCircleWheel;
- }
- void ivpark_simple::dRever2Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- (void)fSpeed;
- static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- int64_t nstoptime_ms = 3000; //every stop time is 3
- if(mlastvehstate != xvehstate)
- {
- nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- }
- mlastvehstate = xvehstate;
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- nshift = 2;
- if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
- {
- fdSpeed = 0;
- fdSecSpeed = 0;
- fAcc = -0.5;
- fWheel = 0;
- return;
- }
- Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
- Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::Compute00().dTpoint2.gps_y, maimgps);
- double fdis = pt1.x - pt2.x;
- if(fdis>-1.0)
- {
- fAcc = 0.0; //acc calcutale by pid
- fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- if((fSpeed>0.3)&&(fabs(fdis)>0.3))
- {
- fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- fAcc = -0.5;
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- xvehstate = dRever3;
- }
- }
- fWheel = 0;
- }
- void ivpark_simple::dRever3Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
- {
- (void)fSpeed;
- static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- int64_t nstoptime_ms = 3000; //every stop time is 3
- if(mlastvehstate != xvehstate)
- {
- nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- }
- mlastvehstate = xvehstate;
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
- nshift = 2;
- if(abs(nnow - nstoptiming_ms) < nstoptime_ms) //stop 3s, change wheel when stop
- {
- fdSpeed = 0;
- fdSecSpeed = 0;
- fAcc = -0.5;
- fWheel = iv::decition::Compute00().bocheAngle*16.5 *1.0;
- mCircleWheel = fWheel;
- return;
- }
- Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
- Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint3.gps_x,iv::decition::Compute00().dTpoint3.gps_y, maimgps);
- double fdis = fabs(pt1.x - pt2.x);
- if(fdis>1.0)
- {
- fAcc = 0.0; //acc calcutale by pid
- fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- if((fSpeed>0.3)&&(fabs(fdis)>0.3))
- {
- fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- }
- else
- {
- fAcc = -0.5;
- fdSpeed = 0.0;
- fdSecSpeed = fdSpeed/3.6;
- xvehstate = reverseDirect;
- }
- }
- fWheel = mCircleWheel;
- }
|