1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667366836693670367136723673367436753676367736783679368036813682368336843685368636873688368936903691369236933694369536963697369836993700370137023703370437053706370737083709371037113712371337143715371637173718371937203721372237233724372537263727372837293730373137323733373437353736373737383739374037413742374337443745374637473748374937503751375237533754375537563757375837593760376137623763376437653766376737683769377037713772377337743775377637773778377937803781378237833784378537863787378837893790379137923793379437953796379737983799380038013802380338043805380638073808380938103811381238133814381538163817381838193820382138223823382438253826382738283829383038313832383338343835383638373838383938403841384238433844384538463847384838493850385138523853385438553856385738583859386038613862386338643865386638673868386938703871387238733874387538763877387838793880388138823883388438853886388738883889389038913892389338943895389638973898389939003901390239033904390539063907390839093910391139123913391439153916391739183919392039213922392339243925392639273928392939303931393239333934393539363937393839393940394139423943394439453946394739483949395039513952395339543955395639573958395939603961396239633964396539663967396839693970397139723973397439753976397739783979398039813982398339843985398639873988398939903991399239933994399539963997399839994000400140024003400440054006400740084009401040114012401340144015401640174018401940204021402240234024402540264027402840294030403140324033403440354036403740384039404040414042404340444045404640474048404940504051405240534054405540564057405840594060406140624063406440654066406740684069407040714072407340744075407640774078407940804081408240834084408540864087408840894090409140924093409440954096409740984099410041014102410341044105410641074108410941104111411241134114411541164117411841194120412141224123412441254126412741284129413041314132413341344135413641374138413941404141414241434144414541464147414841494150415141524153415441554156415741584159416041614162416341644165416641674168 |
- #include "ivdecision_brain.h"
- #include <QCoreApplication>
- #include "common/obs_predict.h"
- #include "ivlog.h"
- #include "xmlparam.h"
- extern iv::Ivlog * givlog;
- bool handPark;
- long handParkTime;
- bool rapidStop;
- int gpsMissCount;
- bool changeRoad;
- double avoidX;
- bool parkBesideRoad;
- double steerSpeed;
- bool transferPieriod;
- bool transferPieriod2;
- double traceDevi;
- using namespace iv::decition;
- namespace iv {
- ivdecision_brain::ivdecision_brain()
- {
- mvehState = VehState::normalRun;
- loadxml();
- }
- int ivdecision_brain::getdecision(brain::decition &xdecition,iv::brain::brainstate & xbs)
- {
- static qint64 nlastdecisiontime = 0;
- static qint64 nLastMapUpdate = 0;
- iv::GPSData now_gps_ins;
- std::shared_ptr<iv::hmi::hmimsg> hmi_ptr;
- if(GetHMImsg(hmi_ptr) == true)
- {
- ServiceCarStatus.mbRunPause = hmi_ptr->mbpause();
- if(hmi_ptr->mbbochemode())
- {
- ServiceCarStatus.bocheMode = true;
- }
- ServiceCarStatus.busmode = hmi_ptr->mbbusmode();
- }
- if(ServiceCarStatus.mbRunPause)
- {
- return 0;
- }
- if(GetGPS(now_gps_ins) == false)
- {
- return -1; //No GPS Position
- }
- if(IsMAPUpdate(nLastMapUpdate))
- {
- GetMAP(mgpsMapLine,nLastMapUpdate);
- mbisFirstRun = true;
- }
- iv::LidarGridPtr lidarptr;
- GetLIDARGrid(lidarptr);
- std::vector<iv::Perception::PerceptionOutput> xvectorper;
- iv::TrafficLight xtrafficlight;
- std::shared_ptr<iv::vbox::vbox> xvbox_ptr;
- if(Getvboxmsg(xvbox_ptr))
- {
- xtrafficlight.leftColor=xvbox_ptr->st_left();
- xtrafficlight.rightColor=xvbox_ptr->st_right();
- xtrafficlight.straightColor=xvbox_ptr->st_straight();
- xtrafficlight.uturnColor=xvbox_ptr->st_turn();
- xtrafficlight.leftTime=xvbox_ptr->time_left();
- xtrafficlight.rightTime=xvbox_ptr->time_right();
- xtrafficlight.straightTime=xvbox_ptr->time_straight();
- xtrafficlight.uturnTime=xvbox_ptr->time_turn();
- }
- updatev2x();
- updateultra();
- updateradar();
- updategroupinfo();
- if(mgpsMapLine.size()<10)
- {
- return 0;
- }
- iv::decition::Decition decition= getDecideFromGPS(*now_gps_ins,mgpsMapLine,lidarptr,xvectorper,xtrafficlight);
- 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);
- 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((QDateTime::currentMSecsSinceEpoch() - nlastdecisiontime));
- nlastdecisiontime = QDateTime::currentMSecsSinceEpoch();
- return 1;
- }
- void ivdecision_brain::updategroupinfo()
- {
- std::shared_ptr<iv::radio::radio_send> groupmsg_ptr;
- if(false == Getp900(groupmsg_ptr))
- {
- return;
- }
- ServiceCarStatus.group_server_status = groupmsg_ptr->server_status();
- ServiceCarStatus.group_comm_speed= (float)groupmsg_ptr->car_control_speed();
- ServiceCarStatus.group_state= groupmsg_ptr->cmd_mode();
- if(ServiceCarStatus.group_state!=2){
- ServiceCarStatus.group_comm_speed=0;
- }
- }
- void ivdecision_brain::updatechassistocarstatus()
- {
- std::shared_ptr<iv::chassis> xchassis_ptr;
- if(false == Getchassis(xchassis_ptr))
- {
- return;
- }
- ServiceCarStatus.epb = xchassis_ptr->epbfault();
- ServiceCarStatus.grade = xchassis_ptr->shift();
- ServiceCarStatus.driverMode = xchassis_ptr->drivemode();
- if(xchassis_ptr->has_epsmode()){
- ServiceCarStatus.epsMode = xchassis_ptr->epsmode();
- ServiceCarStatus.receiveEps=true;
- if(ServiceCarStatus.epsMode == 0)
- {
- ServiceCarStatus.mbRunPause = true;
- }
- }
- if(xchassis_ptr->has_torque())
- {
- ServiceCarStatus.torque_st = xchassis_ptr->torque();
- if(ServiceCarStatus.msysparam.mvehtype=="bus"){
- ServiceCarStatus.torque_st = xchassis_ptr->torque()*0.4;
- }
- std::cout<<"******* xchassis.torque:"<< xchassis_ptr->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_ptr->has_engine_speed())
- {
- ServiceCarStatus.engine_speed = xchassis_ptr->engine_speed();
- std::cout<<"******* xchassis.engine_speed:"<< xchassis_ptr->engine_speed()<<std::endl;
- }
- }
- void ivdecision_brain::loadxml()
- {
- 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");
- }
- void ivdecision_brain::updatev2x()
- {
- std::shared_ptr<iv::v2x::v2x> xv2x_ptr;
- if(false == Getv2xmsg(xv2x_ptr))
- {
- return;
- }
- ServiceCarStatus.stationCmd.received=true;
- ServiceCarStatus.stationCmd.has_carID=xv2x_ptr->has_carid();
- if(ServiceCarStatus.stationCmd.has_carID)
- {
- ServiceCarStatus.stationCmd.carID=xv2x_ptr->carid();
- }
- ServiceCarStatus.stationCmd.has_carMode=xv2x_ptr->has_carmode();
- if(ServiceCarStatus.stationCmd.has_carMode)
- {
- ServiceCarStatus.stationCmd.carMode=xv2x_ptr->carmode();
- qDebug("ServiceCarStatus.stationCmd.carMode:",ServiceCarStatus.stationCmd.carMode);
- givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.carMode:%d",ServiceCarStatus.stationCmd.carMode);
- }
- ServiceCarStatus.stationCmd.has_emergencyStop=xv2x_ptr->has_emergencystop();
- if(ServiceCarStatus.stationCmd.has_emergencyStop)
- {
- ServiceCarStatus.stationCmd.emergencyStop=xv2x_ptr->emergencystop();
- qDebug("ServiceCarStatus.stationCmd.emergencyStop:",ServiceCarStatus.stationCmd.emergencyStop);
- givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.emergencyStop:%d",ServiceCarStatus.stationCmd.emergencyStop);
- }
- ServiceCarStatus.stationCmd.has_stationStop=xv2x_ptr->has_stationstop();
- if(ServiceCarStatus.stationCmd.has_stationStop)
- {
- ServiceCarStatus.stationCmd.stationStop=xv2x_ptr->stationstop();
- qDebug("ServiceCarStatus.stationCmd.stationStop:",ServiceCarStatus.stationCmd.stationStop);
- givlog->debug("brain_v2x","ServiceCarStatus.stationCmd.stationStop:%d",ServiceCarStatus.stationCmd.stationStop);
- }
- int num=xv2x_ptr->stationid_size();
- if(num>0)
- {
- ServiceCarStatus.stationCmd.stationTotalNum=num;
- for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
- {
- ServiceCarStatus.stationCmd.stationGps[i].gps_lat=xv2x_ptr->stgps(i).lat();
- ServiceCarStatus.stationCmd.stationGps[i].gps_lng=xv2x_ptr->stgps(i).lon();
- qDebug("stationGps: %d, lat: %.7f, lon: %.7f", xv2x_ptr->stationid(i), ServiceCarStatus.stationCmd.stationGps[i].gps_lat, ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
- givlog->debug("brain_v2x","stationGps: %d, lat: %.7f, lon: %.7f",
- xv2x_ptr->stationid(i),ServiceCarStatus.stationCmd.stationGps[i].gps_lat,
- ServiceCarStatus.stationCmd.stationGps[i].gps_lng);
- }
- ServiceCarStatus.mbRunPause=false;
- }
- }
- void ivdecision_brain::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;
- }
- void ivdecision_brain::updateultra()
- {
- std::shared_ptr<iv::ultrasonic::ultrasonic> xultra_ptr;
- if(false == Getultrasonic(xultra_ptr))
- {
- return;
- }
- ServiceCarStatus.ultraDistance[1]=xultra_ptr->sigobjdist_flcorner();
- ServiceCarStatus.ultraDistance[2]=xultra_ptr->sigobjdist_flmiddle();
- ServiceCarStatus.ultraDistance[3]=xultra_ptr->sigobjdist_flside();
- ServiceCarStatus.ultraDistance[4]=xultra_ptr->sigobjdist_frcorner();
- ServiceCarStatus.ultraDistance[5]=xultra_ptr->sigobjdist_frmiddle();
- ServiceCarStatus.ultraDistance[6]=xultra_ptr->sigobjdist_frside();
- ServiceCarStatus.ultraDistance[7]=xultra_ptr->sigobjdist_rlcorner();
- ServiceCarStatus.ultraDistance[8]=xultra_ptr->sigobjdist_rlmiddle();
- ServiceCarStatus.ultraDistance[9]=xultra_ptr->sigobjdist_rlside();
- ServiceCarStatus.ultraDistance[10]=xultra_ptr->sigobjdist_rrcorner();
- ServiceCarStatus.ultraDistance[11]=xultra_ptr->sigobjdist_rrmiddle();
- ServiceCarStatus.ultraDistance[12]=xultra_ptr->sigobjdist_rrside();
- }
- void ivdecision_brain::updateradar()
- {
- std::shared_ptr<iv::radar::radarobjectarray> xradar_ptr
- = std::shared_ptr<iv::radar::radarobjectarray>(new iv::radar::radarobjectarray);
- unsigned int i;
- if(false == GetRADAR(xradar_ptr))
- {
- for(i=0;i<64;i++)
- {
- ServiceCarStatus.obs_radar[i].valid = false;
- }
- return;
- }
- for(i=0;i<64;i++)
- {
- iv::radar::radarobject * pradarobj = xradar_ptr->mutable_obj(i);
- ServiceCarStatus.obs_radar[i].valid = pradarobj->bvalid();
- ServiceCarStatus.obs_radar[i].nomal_x = pradarobj->x();
- ServiceCarStatus.obs_radar[i].nomal_y = pradarobj->y();
- ServiceCarStatus.obs_radar[i].speed_relative = pradarobj->vel();
- ServiceCarStatus.obs_radar[i].speed_x = pradarobj->vx();
- ServiceCarStatus.obs_radar[i].speed_y = pradarobj->vy();
- }
- }
- //int ivdecision_brain::getdecision_normal(brain::decition &xdecition)
- //{
- // iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- // double obsDistance = -1;
- // int esrLineIndex = -1;
- // double lidarDistance = -1;
- // int roadPre = -1;
- // return 0;
- //}
- //std::vector<iv::Perception::PerceptionOutput> lidar_per,
- iv::decition::Decition ivdecision_brain::decision_reverseCar(GPS_INS now_gps_ins)
- {
- iv::decition::Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
- vehState = reversing;
- qiedianCount=true;
- return decision_reversing(now_gps_ins);
- }
- iv::decition::Decition ivdecision_brain::decision_reversing(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- double dis = GetDistance(now_gps_ins, iv::decition::Compute00().nearTpoint);
- if (dis<2.0)//0.5
- {
- vehState = reverseCircle;
- qiedianCount = true;
- cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
- return decision_reverseCircle(now_gps_ins);
- }
- 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;
- }
- }
- iv::decition::Decition ivdecision_brain::decision_reverseCircle(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- double dis = GetDistance(now_gps_ins, iv::decition::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;
- return decision_reverseDirect(now_gps_ins);
- }
- 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 = iv::decition::Compute00().bocheAngle*16.5;
- gps_decition->wheel_angle = 0 - controlAng*1.05;
- cout<<"farTpointDistance================"<<dis<<endl;
- return gps_decition;
- }
- }
- iv::decition::Decition ivdecision_brain::decision_reverseDirect(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- 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;
- }
- }
- iv::decition::Decition ivdecision_brain::decision_reverseArr(iv::GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- 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;
- }
- iv::decition::Decition ivdecision_brain::decision_dRever(GPS_INS now_gps_ins)
- {
- GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
- iv::decition::Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
- vehState = dRever0;
- qiedianCount=true;
- std::cout<<"enter side boche mode"<<std::endl;
- return decision_dRever0(now_gps_ins);
- }
- iv::decition::Decition ivdecision_brain::decision_dRever0(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint0);
- if (dis<2.0)
- {
- vehState = dRever1;
- qiedianCount = true;
- cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
- return decision_dRever1(now_gps_ins);
- }
- 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;
- }
- }
- iv::decition::Decition ivdecision_brain::decision_dRever1(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint1);
- if(dis<2.0)
- {
- vehState = dRever2;
- qiedianCount = true;
- cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
- return decision_dRever2(now_gps_ins);
- }
- 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 = iv::decition::Compute00().dBocheAngle*16.5;
- gps_decition->wheel_angle = 0 - controlAng;
- cout<<"farTpointDistance================"<<dis<<endl;
- return gps_decition;
- }
- }
- iv::decition::Decition ivdecision_brain::decision_dRever2(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint2);
- Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
- Point2D pt2 = Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::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;
- return decision_dRever3(now_gps_ins);
- }
- 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;
- }
- }
- iv::decition::Decition ivdecision_brain::decision_dRever3(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- double dis = GetDistance(now_gps_ins, iv::decition::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;
- return decision_dRever4(now_gps_ins);
- }
- 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-iv::decition::Compute00().dBocheAngle*16.5;
- gps_decition->wheel_angle = 0 - controlAng*0.95;
- cout<<"farTpointDistance================"<<dis<<endl;
- return gps_decition;
- }
- }
- iv::decition::Decition ivdecision_brain::decision_dRever4(GPS_INS now_gps_ins)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- // 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;
- }
- }
- void ivdecision_brain::decision_firstRun(GPS_INS now_gps_ins,
- const std::vector<GPSData> & gpsMapLine)
- {
- if (isFirstRun)
- {
- initMethods();
- vehState = normalRun;
- startAvoid_gps_ins = now_gps_ins;
- init();
- PathPoint = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
- lastGpsIndex,
- minDis,
- maxAngle);
- lastGpsIndex = PathPoint;
- if(ServiceCarStatus.speed_control==true){
- iv::decition::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 = iv::decition::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;
- }
- }
- int ivdecision_brain::decision_ParkCalc(GPS_INS now_gps_ins,iv::decition::Decition & gps_decition)
- {
- //sidePark
- if(ServiceCarStatus.mnParkType==1){
- if( vehState!=dRever && vehState!=dRever0 && vehState!=dRever1 && vehState!=dRever2 && vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
- int bocheEN=iv::decition::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 1;
- }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=iv::decition::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 1;
- }else{
- if(trumpet()>3000){
- trumpetFirstCount=true;
- vehState=reverseCar;
- }
- }
- // ServiceCarStatus.bocheMode=false;
- }
- }
- return 0;
- }
- void ivdecision_brain::decision_readyZhucheMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
- {
- 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;
- }
- }
- }
- void ivdecision_brain::decision_readyParkMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
- {
- 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;
- }
- }
- }
- void ivdecision_brain::decision_speedctrl(Decition &gps_decition,const std::vector<GPSData> & gpsMapLine)
- {
- 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;
- }
- //std::vector<iv::Perception::PerceptionOutput> lidar_per,
- iv::decition::Decition ivdecision_brain::getDecideFromGPS(GPS_INS now_gps_ins,
- const std::vector<GPSData> gpsMapLine,
- iv::LidarGridPtr lidarGridPtr,
- std::vector<iv::Perception::PerceptionOutput> lidar_per,
- iv::TrafficLight trafficLight)
- {
- iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
- //如果useFrenet、useOldAvoid两者不互为相异,则采用原来的方法。“^”为异或运算符。
- if(!(useFrenet^useOldAvoid)){
- useFrenet = false;
- useOldAvoid = true;
- }
- if (isFirstRun)
- {
- decision_firstRun(now_gps_ins,gpsMapLine);
- }
- 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);
- }
- 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;
- }
- 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;
- }
- //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;
- realspeed = now_gps_ins.speed;
- secSpeed = realspeed / 3.6;
- if(decision_ParkCalc(now_gps_ins,gps_decition) == 1)
- {
- return gps_decition;
- }
- switch (vehState) {
- // ChuiZhiTingChe
- case reverseCar:
- return decision_reverseCar(now_gps_ins);
- break;
- case reversing:
- return decision_reversing(now_gps_ins);
- break;
- case reverseCircle:
- return decision_reverseCircle(now_gps_ins);
- break;
- case reverseDirect:
- return decision_reverseDirect(now_gps_ins);
- break;
- case reverseArr:
- return decision_reverseArr(now_gps_ins);
- break;
- //ceFangWeiBoChe
- case dRever:
- return decision_dRever(now_gps_ins);
- break;
- case dRever0:
- return decision_dRever0(now_gps_ins);
- break;
- case dRever1:
- return decision_dRever1(now_gps_ins);
- break;
- case dRever2:
- return decision_dRever2(now_gps_ins);
- break;
- case dRever3:
- return decision_dRever3(now_gps_ins);
- break;
- case dRever4:
- return decision_dRever4(now_gps_ins);
- break;
- default:
- break;
- }
- 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 = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
- // if(PathPoint<0){
- PathPoint = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
- // }
- }
- if (PathPoint<0)
- {
- minDecelerate=-1.0;
- phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
- return gps_decition;
- }
- 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;
- }
- 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)
- {
- decision_readyZhucheMode(now_gps_ins,gpsMapLine);
- }
- if (readyParkMode)
- {
- decision_readyParkMode(now_gps_ins,gpsMapLine);
- }
- decision_speedctrl(gps_decition,gpsMapLine);
- 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
- 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 = iv::decition::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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> ivdecision_brain::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
- vector<iv::Point2D> trace;
- // int PathPoint = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, 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;
- zhuchePointNum = index;
- }
- if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
- {
- readyZhucheMode = true;
- zhuchePointNum = index;
- }
- //csvv7
- if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
- {
- readyParkMode = true;
- finishPointNum = index;
- }
- // if (pt.v1 == 22 || pt.v1 == 23)
- // {
- // readyParkMode = true;
- // 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> ivdecision_brain::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 + 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;
- 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;
- trace.push_back(ptRight);
- }
- }
- return trace;
- }
- double ivdecision_brain::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
- double angle=0;
- if ( abs(minDis) < 20 && abs(maxAngle) < 100)
- {
- // angle = iv::decition::iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
- pid_Controller->getControlDecition(gpsIns, gpsTrace, -1, -1, -1, true, false, &decition);
- angle= decition->wheel_angle;
- }
- return angle;
- }
- double ivdecision_brain::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 ivdecision_brain::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
- //
- // if (!obsRadars.empty())
- // {
- // esrIndex = iv::decition::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 ivdecision_brain::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
- int esrPathpoint;
- esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint,xiuzhengCs);
- 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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
- if (lidarGridPtr == NULL)
- {
- lidarDistanceAvoid = lastLidarDisAvoid;
- }
- else {
- obsPointAvoid = iv::decition::Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr,lidarDistanceAvoid);
- lastLidarDisAvoid = lidarDistanceAvoid;
- }
- std::cout << "\nLidarAvoid距离:%f\n" << lidarDistanceAvoid << std::endl;
- getEsrObsDistanceAvoid();
- //lidarDistanceAvoid = -1; //20200103 apollo_fu
- if (esrDistanceAvoid>0 && lidarDistanceAvoid > 0)
- {
- if (lidarDistanceAvoid >= esrDistanceAvoid)
- {
- obsDistanceAvoid = esrDistanceAvoid;
- obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
- }
- else if (!ServiceCarStatus.obs_radar.empty())
- {
- obsDistanceAvoid = lidarDistanceAvoid;
- obsSpeedAvoid = iv::decition::Compute00().getObsSpeed(obsPointAvoid, secSpeed);
- std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
- }
- else
- {
- obsDistanceAvoid = 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 (lidarDistanceAvoid > 0)
- {
- obsDistanceAvoid = lidarDistanceAvoid;
- obsSpeedAvoid = iv::decition::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 (obsDistanceAvoid <0 && obsLostTimeAvoid<4)
- {
- obsDistanceAvoid = lastDistanceAvoid;
- obsLostTimeAvoid++;
- }
- else
- {
- obsLostTimeAvoid = 0;
- lastDistanceAvoid = -1;
- }
- if (obsDistanceAvoid>0)
- {
- lastDistanceAvoid = obsDistanceAvoid;
- }
- std::cout << "\nODSAvoid距离:%f\n" << obsDistanceAvoid << std::endl;
- }
- void ivdecision_brain::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 ivdecision_brain::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 = iv::decition::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 = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
- obsSd = iv::decition::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 = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
- obs = lidarDistance;
- obsSd = iv::decition::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 ivdecision_brain::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 = iv::decition::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 = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
- obsSd = iv::decition::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 = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
- obs = lidarDistance;
- obsSd = iv::decition::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::trumpet() {
- if (trumpetFirstCount)
- {
- trumpetFirstCount = false;
- trumpetLastTime= GetTickCount();
- trumpetTimeSpan = 0.0;
- }
- else
- {
- trumpetStartTime= GetTickCount();
- trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
- trumpetLastTime = trumpetStartTime;
- }
- return trumpetTimeSpan;
- }
- double ivdecision_brain::transferP() {
- if (transferFirstCount)
- {
- transferFirstCount = false;
- transferLastTime= GetTickCount();
- transferTimeSpan = 0.0;
- }
- else
- {
- transferStartTime= GetTickCount();
- transferTimeSpan += transferStartTime - transferLastTime;
- transferLastTime = transferStartTime;
- }
- return transferTimeSpan;
- }
- void ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {
- if (lidarGridPtr == NULL)
- {
- return false;
- // lidarDistance = lastlidarDistance;
- }
- else {
- obsPoint = iv::decition::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 = iv::decition::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 = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
- obsSd = iv::decition::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-M_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 = iv::decition::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-M_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 ivdecision_brain::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
- esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum,xiuzhengCs);
- if (esrIndex != -1)
- {
- esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
- obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
- }
- else {
- esrDistance = -1;
- }
- }
- void ivdecision_brain::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,xiuzhengCs);
- 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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::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 ivdecision_brain::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
- if( gpsMapLine[PathPoint]->mode2==3000){
- if(obsDistance>5){
- obsDistance=200;
- }
- dSpeed=min(dSpeed,5.0);
- }
- }
- float ivdecision_brain::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;
- }
- }
|