ivdecision_brain.cpp 109 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667366836693670367136723673367436753676367736783679368036813682368336843685368636873688368936903691369236933694369536963697369836993700370137023703370437053706370737083709371037113712371337143715371637173718371937203721372237233724372537263727372837293730373137323733373437353736373737383739374037413742374337443745374637473748374937503751
  1. #include "ivdecision_brain.h"
  2. #include "common/obs_predict.h"
  3. #include "ivlog.h"
  4. extern iv::Ivlog * givlog;
  5. bool handPark;
  6. long handParkTime;
  7. bool rapidStop;
  8. int gpsMissCount;
  9. bool changeRoad;
  10. double avoidX;
  11. bool parkBesideRoad;
  12. double steerSpeed;
  13. bool transferPieriod;
  14. bool transferPieriod2;
  15. double traceDevi;
  16. using namespace iv::decition;
  17. namespace iv {
  18. ivdecision_brain::ivdecision_brain()
  19. {
  20. mvehState = VehState::normalRun;
  21. }
  22. int ivdecision_brain::getdecision(brain::decition &xdecition)
  23. {
  24. static qint64 nLastMapUpdate = 0;
  25. iv::GPSData now_gps_ins;
  26. if(GetGPS(now_gps_ins) == false)
  27. {
  28. return -1; //No GPS Position
  29. }
  30. if(IsMAPUpdate(nLastMapUpdate))
  31. {
  32. GetMAP(mgpsMapLine,nLastMapUpdate);
  33. mbisFirstRun = true;
  34. }
  35. if(mbisFirstRun)
  36. {
  37. //Init
  38. }
  39. switch (mvehState) {
  40. case VehState::normalRun:
  41. // return getdecision_normal(xdecition);
  42. break;
  43. default:
  44. break;
  45. }
  46. return -1000;//mode error.
  47. }
  48. //int ivdecision_brain::getdecision_normal(brain::decition &xdecition)
  49. //{
  50. // iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  51. // double obsDistance = -1;
  52. // int esrLineIndex = -1;
  53. // double lidarDistance = -1;
  54. // int roadPre = -1;
  55. // return 0;
  56. //}
  57. //std::vector<iv::Perception::PerceptionOutput> lidar_per,
  58. iv::decition::Decition ivdecision_brain::decision_reverseCar(GPS_INS now_gps_ins)
  59. {
  60. iv::decition::Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
  61. vehState = reversing;
  62. qiedianCount=true;
  63. return decision_reversing(now_gps_ins);
  64. }
  65. iv::decition::Decition ivdecision_brain::decision_reversing(GPS_INS now_gps_ins)
  66. {
  67. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  68. double dis = GetDistance(now_gps_ins, iv::decition::Compute00().nearTpoint);
  69. if (dis<2.0)//0.5
  70. {
  71. vehState = reverseCircle;
  72. qiedianCount = true;
  73. cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
  74. return decision_reverseCircle(now_gps_ins);
  75. }
  76. else {
  77. controlAng = 0;
  78. dSpeed = 2;
  79. dSecSpeed = dSpeed / 3.6;
  80. gps_decition->wheel_angle = 0;
  81. gps_decition->speed = dSpeed;
  82. obsDistance=-1;
  83. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  84. // gps_decition->accelerator = 0;
  85. return gps_decition;
  86. }
  87. }
  88. iv::decition::Decition ivdecision_brain::decision_reverseCircle(GPS_INS now_gps_ins)
  89. {
  90. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  91. double dis = GetDistance(now_gps_ins, iv::decition::Compute00().farTpoint);
  92. double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
  93. if((((angdis<5)||(angdis>355)))&&(dis<3.0))
  94. // if((((angdis<4)||(angdis>356)))&&(dis<2.0))
  95. {
  96. vehState = reverseDirect;
  97. qiedianCount = true;
  98. cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
  99. return decision_reverseDirect(now_gps_ins);
  100. }
  101. else {
  102. if (qiedianCount && trumpet()<0)
  103. // if (qiedianCount && trumpet()<1500)
  104. {
  105. // gps_decition->brake = 10;
  106. // gps_decition->torque = 0;
  107. dSpeed=0;
  108. minDecelerate=min(minDecelerate,-0.5f);
  109. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  110. }
  111. else
  112. {
  113. qiedianCount = false;
  114. trumpetFirstCount = true;
  115. dSpeed = 2;
  116. dSecSpeed = dSpeed / 3.6;
  117. gps_decition->speed = dSpeed;
  118. obsDistance=-1;
  119. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  120. }
  121. controlAng = iv::decition::Compute00().bocheAngle*16.5;
  122. gps_decition->wheel_angle = 0 - controlAng*1.05;
  123. cout<<"farTpointDistance================"<<dis<<endl;
  124. return gps_decition;
  125. }
  126. }
  127. iv::decition::Decition ivdecision_brain::decision_reverseDirect(GPS_INS now_gps_ins)
  128. {
  129. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  130. Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
  131. if ((pt.y)<0.5)
  132. {
  133. qiedianCount=true;
  134. vehState=reverseArr;
  135. cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
  136. // gps_decition->accelerator = -3;
  137. // gps_decition->brake = 10;
  138. // gps_decition->torque = 0;
  139. gps_decition->wheel_angle=0;
  140. dSpeed=0;
  141. minDecelerate=min(minDecelerate,-0.5f);
  142. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  143. return gps_decition;
  144. }
  145. else {
  146. //if (qiedianCount && trumpet()<0)
  147. if (qiedianCount && trumpet()<1000)
  148. {
  149. // gps_decition->brake = 10;
  150. // gps_decition->torque = 0;
  151. dSpeed=0;
  152. minDecelerate=min(minDecelerate,-0.5f);
  153. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  154. }
  155. else
  156. {
  157. qiedianCount = false;
  158. trumpetFirstCount = true;
  159. dSpeed = 1;
  160. dSecSpeed = dSpeed / 3.6;
  161. gps_decition->speed = dSpeed;
  162. obsDistance=-1;
  163. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  164. }
  165. controlAng = 0;
  166. gps_decition->wheel_angle = 0;
  167. return gps_decition;
  168. }
  169. }
  170. iv::decition::Decition ivdecision_brain::decision_reverseArr(iv::GPS_INS now_gps_ins)
  171. {
  172. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  173. ServiceCarStatus.bocheMode=false;
  174. if (qiedianCount && trumpet()<1500)
  175. {
  176. // gps_decition->brake = 10;
  177. // gps_decition->torque = 0;
  178. dSpeed=0;
  179. minDecelerate=min(minDecelerate,-0.5f);
  180. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  181. ServiceCarStatus.bocheMode=false;
  182. }
  183. else
  184. {
  185. qiedianCount = false;
  186. trumpetFirstCount = true;
  187. ServiceCarStatus.bocheEnable=0;
  188. vehState=normalRun;
  189. ServiceCarStatus.mbRunPause=true;
  190. ServiceCarStatus.mnBocheComplete=100;
  191. cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
  192. }
  193. gps_decition->wheel_angle = 0 ;
  194. return gps_decition;
  195. }
  196. iv::decition::Decition ivdecision_brain::decision_dRever(GPS_INS now_gps_ins)
  197. {
  198. GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
  199. iv::decition::Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
  200. vehState = dRever0;
  201. qiedianCount=true;
  202. std::cout<<"enter side boche mode"<<std::endl;
  203. return decision_dRever0(now_gps_ins);
  204. }
  205. iv::decition::Decition ivdecision_brain::decision_dRever0(GPS_INS now_gps_ins)
  206. {
  207. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  208. double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint0);
  209. if (dis<2.0)
  210. {
  211. vehState = dRever1;
  212. qiedianCount = true;
  213. cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
  214. return decision_dRever1(now_gps_ins);
  215. }
  216. else {
  217. controlAng = 0;
  218. dSpeed = 2;
  219. dSecSpeed = dSpeed / 3.6;
  220. gps_decition->wheel_angle = 0;
  221. gps_decition->speed = dSpeed;
  222. obsDistance=-1;
  223. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  224. // gps_decition->accelerator = 0;
  225. return gps_decition;
  226. }
  227. }
  228. iv::decition::Decition ivdecision_brain::decision_dRever1(GPS_INS now_gps_ins)
  229. {
  230. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  231. double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint1);
  232. if(dis<2.0)
  233. {
  234. vehState = dRever2;
  235. qiedianCount = true;
  236. cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
  237. return decision_dRever2(now_gps_ins);
  238. }
  239. else {
  240. if (qiedianCount && trumpet()<1000)
  241. {
  242. // gps_decition->brake = 10;
  243. // gps_decition->torque = 0;
  244. dSpeed=0;
  245. minDecelerate=min(minDecelerate,-0.5f);
  246. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  247. }
  248. else
  249. {
  250. qiedianCount = false;
  251. trumpetFirstCount = true;
  252. dSpeed = 2;
  253. dSecSpeed = dSpeed / 3.6;
  254. gps_decition->speed = dSpeed;
  255. obsDistance=-1;
  256. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  257. }
  258. controlAng = iv::decition::Compute00().dBocheAngle*16.5;
  259. gps_decition->wheel_angle = 0 - controlAng;
  260. cout<<"farTpointDistance================"<<dis<<endl;
  261. return gps_decition;
  262. }
  263. }
  264. iv::decition::Decition ivdecision_brain::decision_dRever2(GPS_INS now_gps_ins)
  265. {
  266. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  267. double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint2);
  268. Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
  269. Point2D pt2 = Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::Compute00().dTpoint2.gps_y, aim_gps_ins);
  270. double xx= (pt1.x-pt2.x);
  271. // if(xx>0)
  272. if(xx>-0.5)
  273. {
  274. GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
  275. Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
  276. double xxx=ptt.x;
  277. double yyy =ptt.y;
  278. // if(xxx<-1.5||xx>1){
  279. // int a=0;
  280. // }
  281. vehState = dRever3;
  282. qiedianCount = true;
  283. cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
  284. cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
  285. cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
  286. return decision_dRever3(now_gps_ins);
  287. }
  288. else {
  289. if (qiedianCount && trumpet()<1000)
  290. {
  291. /* gps_decition->brake = 10;
  292. gps_decition->torque = 0; */
  293. dSpeed=0;
  294. minDecelerate=min(minDecelerate,-0.5f);
  295. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  296. }
  297. else
  298. {
  299. qiedianCount = false;
  300. trumpetFirstCount = true;
  301. dSpeed = 2;
  302. dSecSpeed = dSpeed / 3.6;
  303. gps_decition->speed = dSpeed;
  304. obsDistance=-1;
  305. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  306. }
  307. gps_decition->wheel_angle = 0 ;
  308. cout<<"farTpointDistance================"<<dis<<endl;
  309. return gps_decition;
  310. }
  311. }
  312. iv::decition::Decition ivdecision_brain::decision_dRever3(GPS_INS now_gps_ins)
  313. {
  314. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  315. double dis = GetDistance(now_gps_ins, iv::decition::Compute00().dTpoint3);
  316. double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
  317. if((((angdis<5)||(angdis>355)))&&(dis<10.0))
  318. {
  319. vehState = dRever4;
  320. qiedianCount = true;
  321. cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
  322. return decision_dRever4(now_gps_ins);
  323. }
  324. else {
  325. if (qiedianCount && trumpet()<1000)
  326. {
  327. // gps_decition->brake = 10;
  328. // gps_decition->torque = 0;
  329. dSpeed=0;
  330. minDecelerate=min(minDecelerate,-0.5f);
  331. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  332. }
  333. else
  334. {
  335. qiedianCount = false;
  336. trumpetFirstCount = true;
  337. dSpeed = 2;
  338. dSecSpeed = dSpeed / 3.6;
  339. gps_decition->speed = dSpeed;
  340. obsDistance=-1;
  341. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  342. }
  343. controlAng = 0-iv::decition::Compute00().dBocheAngle*16.5;
  344. gps_decition->wheel_angle = 0 - controlAng*0.95;
  345. cout<<"farTpointDistance================"<<dis<<endl;
  346. return gps_decition;
  347. }
  348. }
  349. iv::decition::Decition ivdecision_brain::decision_dRever4(GPS_INS now_gps_ins)
  350. {
  351. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  352. // double dis = GetDistance(now_gps_ins, aim_gps_ins);
  353. Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
  354. if ((pt.y)<0.5)
  355. {
  356. qiedianCount=true;
  357. vehState=reverseArr;
  358. cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
  359. // gps_decition->accelerator = -3;
  360. // gps_decition->brake =10 ;
  361. dSpeed=0;
  362. minDecelerate=min(minDecelerate,-0.5f);
  363. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  364. gps_decition->wheel_angle=0;
  365. return gps_decition;
  366. }
  367. else {
  368. //if (qiedianCount && trumpet()<0)
  369. if (qiedianCount && trumpet()<1000)
  370. {
  371. // gps_decition->brake = 10;
  372. // gps_decition->torque = 0;
  373. dSpeed=0;
  374. minDecelerate=min(minDecelerate,-0.5f);
  375. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  376. }
  377. else
  378. {
  379. qiedianCount = false;
  380. trumpetFirstCount = true;
  381. dSpeed = 2;
  382. dSecSpeed = dSpeed / 3.6;
  383. gps_decition->speed = dSpeed;
  384. obsDistance=-1;
  385. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  386. }
  387. controlAng = 0;
  388. gps_decition->wheel_angle = 0;
  389. return gps_decition;
  390. }
  391. }
  392. void ivdecision_brain::decision_firstRun(GPS_INS now_gps_ins,
  393. const std::vector<GPSData> & gpsMapLine)
  394. {
  395. if (isFirstRun)
  396. {
  397. initMethods();
  398. vehState = normalRun;
  399. startAvoid_gps_ins = now_gps_ins;
  400. init();
  401. PathPoint = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
  402. lastGpsIndex,
  403. minDis,
  404. maxAngle);
  405. lastGpsIndex = PathPoint;
  406. if(ServiceCarStatus.speed_control==true){
  407. iv::decition::Compute00().getDesiredSpeed(gpsMapLine); //add by zj
  408. }
  409. // ServiceCarStatus.carState = 1;
  410. // roadOri = gpsMapLine[PathPoint]->roadOri;
  411. // roadNow = roadOri;
  412. // roadSum = gpsMapLine[PathPoint]->roadSum;
  413. // busMode = false;
  414. // vehState = dRever;
  415. double dis = iv::decition::GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
  416. if(dis<15){
  417. circleMode=true; //201200102
  418. }else{
  419. circleMode=false;
  420. }
  421. // circleMode = true;
  422. getV2XTrafficPositionVector(gpsMapLine);
  423. // group_ori_gps=*gpsMapLine[0];
  424. ServiceCarStatus.bocheMode=false;
  425. ServiceCarStatus.daocheMode=false;
  426. parkMode=false;
  427. readyParkMode=false;
  428. finishPointNum=-1;
  429. isFirstRun = false;
  430. }
  431. }
  432. int ivdecision_brain::decision_ParkCalc(GPS_INS now_gps_ins,iv::decition::Decition & gps_decition)
  433. {
  434. //sidePark
  435. if(ServiceCarStatus.mnParkType==1){
  436. if( vehState!=dRever && vehState!=dRever0 && vehState!=dRever1 && vehState!=dRever2 && vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr ){
  437. int bocheEN=iv::decition::Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
  438. if(bocheEN==1){
  439. ServiceCarStatus.bocheEnable=1;
  440. }else if(bocheEN==0){
  441. ServiceCarStatus.bocheEnable=0;
  442. }
  443. }else{
  444. ServiceCarStatus.bocheEnable=2;
  445. }
  446. if(ServiceCarStatus.bocheMode && vehState!=dRever && vehState!=dRever0 && vehState!=dRever1 &&
  447. vehState!=dRever2 && vehState!=dRever3 && vehState!=dRever4&& vehState!=reverseArr ){
  448. if(abs(realspeed)>0.1){
  449. dSpeed=0;
  450. minDecelerate=min(minDecelerate,-0.5f);
  451. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  452. gps_decition->wheel_angle=0;
  453. return 1;
  454. }else{
  455. if(trumpet()>3000){
  456. trumpetFirstCount=true;
  457. vehState=dRever;
  458. }
  459. }
  460. // ServiceCarStatus.bocheMode=false;
  461. }
  462. }
  463. //chuizhiPark
  464. if(ServiceCarStatus.mnParkType==0){
  465. if( vehState!=reverseCar && vehState!=reversing && vehState!=reverseCircle && vehState!=reverseDirect && vehState!=reverseArr ){
  466. int bocheEN=iv::decition::Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
  467. if(bocheEN==1){
  468. ServiceCarStatus.bocheEnable=1;
  469. }else if(bocheEN==0){
  470. ServiceCarStatus.bocheEnable=0;
  471. }
  472. }else{
  473. ServiceCarStatus.bocheEnable=2;
  474. }
  475. if(ServiceCarStatus.bocheMode && vehState!=reverseCar && vehState!=reversing && vehState!=reverseCircle && vehState!=reverseDirect&& vehState!=reverseArr ){
  476. if(abs(realspeed)>0.1){
  477. dSpeed=0;
  478. minDecelerate=min(minDecelerate,-0.5f);
  479. phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
  480. gps_decition->wheel_angle=0;
  481. return 1;
  482. }else{
  483. if(trumpet()>3000){
  484. trumpetFirstCount=true;
  485. vehState=reverseCar;
  486. }
  487. }
  488. // ServiceCarStatus.bocheMode=false;
  489. }
  490. }
  491. return 0;
  492. }
  493. void ivdecision_brain::decision_readyZhucheMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
  494. {
  495. cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
  496. cout<<"zhuche point : "<<zhuchePointNum<<endl;
  497. double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
  498. if (dis<35)
  499. {
  500. Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
  501. double zhucheDistance = pt.y;
  502. #if 0
  503. if (dSpeed > 15)
  504. {
  505. dSpeed = 15;
  506. }
  507. if (zhucheDistance < 20 && dis < 25)
  508. {
  509. dSpeed = min(dSpeed, 5.0);
  510. }
  511. #else
  512. if (dSpeed > 12)
  513. {
  514. dSpeed = 12;
  515. }
  516. if (zhucheDistance < 9 && dis < 9)
  517. {
  518. dSpeed = min(dSpeed, 5.0);
  519. }
  520. #endif
  521. if (zhucheDistance < 3.0 && dis < 3.5)
  522. {
  523. dSpeed = min(dSpeed, 5.0);
  524. zhucheMode = true;
  525. readyZhucheMode = false;
  526. cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
  527. //1125
  528. // gps_decition->brake=20;
  529. // gps_decition->accelerator = -3;
  530. // gps_decition->wheel_angle = 0-controlAng;
  531. // gps_decition->speed = 0;
  532. // gps_decition->torque=0;
  533. // return gps_decition;
  534. }
  535. }
  536. }
  537. void ivdecision_brain::decision_readyParkMode(GPS_INS now_gps_ins, const std::vector<GPSData> &gpsMapLine)
  538. {
  539. double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
  540. hasZhuched = true;
  541. if (parkDis < 25)
  542. {
  543. Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
  544. double parkDistance = pt.y;
  545. if (dSpeed > 15)
  546. {
  547. dSpeed = 15;
  548. }
  549. //dSpeed = 15;//////////////////////////////
  550. if (parkDistance < 15 && parkDistance >= 12.5 && parkDis<20)
  551. {
  552. dSpeed = min(dSpeed, 8.0);
  553. }else if (parkDistance < 12.5 && parkDistance >= 8.5 && parkDis<15.0){
  554. dSpeed = min(dSpeed, 5.0);
  555. }else if (parkDistance < 8.5 && parkDistance >= 5.5 && parkDis<9.0){
  556. dSpeed = min(dSpeed, 3.0);
  557. }else if(parkDistance < 5.5 && parkDis<6.0){
  558. dSpeed = min(dSpeed, 1.0);
  559. }
  560. // float stopDis=2;
  561. // if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
  562. // stopDis=1.6;
  563. // } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
  564. // stopDis=1.8;
  565. // }
  566. if (parkDistance < 1.6 && parkDis<2.0)
  567. {
  568. // dSpeed = 0;
  569. parkMode = true;
  570. readyParkMode = false;
  571. }
  572. }
  573. }
  574. void ivdecision_brain::decision_speedctrl(Decition &gps_decition,const std::vector<GPSData> & gpsMapLine)
  575. {
  576. if (gpsMapLine[PathPoint]->roadMode ==0)
  577. {
  578. //dSpeed = min(10.0,dSpeed);
  579. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
  580. gps_decition->leftlamp = false;
  581. gps_decition->rightlamp = false;
  582. }else if (gpsMapLine[PathPoint]->roadMode == 5)
  583. {
  584. //dSpeed = min(8.0,dSpeed);
  585. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
  586. gps_decition->leftlamp = false;
  587. gps_decition->rightlamp = false;
  588. }else if (gpsMapLine[PathPoint]->roadMode == 11)
  589. {
  590. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
  591. gps_decition->leftlamp = false;
  592. gps_decition->rightlamp = false;
  593. }else if (gpsMapLine[PathPoint]->roadMode == 14)
  594. {
  595. //dSpeed = min(8.0,dSpeed);
  596. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
  597. gps_decition->leftlamp = true;
  598. gps_decition->rightlamp = false;
  599. }else if (gpsMapLine[PathPoint]->roadMode == 15)
  600. {
  601. //dSpeed = min(8.0,dSpeed);
  602. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
  603. gps_decition->leftlamp = false;
  604. gps_decition->rightlamp = true;
  605. }else if (gpsMapLine[PathPoint]->roadMode == 16)
  606. {
  607. // dSpeed = min(10.0,dSpeed);
  608. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
  609. //gps_decition->leftlamp = true;
  610. //gps_decition->rightlamp = false;
  611. }else if (gpsMapLine[PathPoint]->roadMode == 17)
  612. {
  613. // dSpeed = min(10.0,dSpeed);
  614. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
  615. //gps_decition->leftlamp = false;
  616. //gps_decition->rightlamp = true;
  617. }else if (gpsMapLine[PathPoint]->roadMode == 18)
  618. {
  619. // dSpeed = min(10.0,dSpeed);
  620. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
  621. gps_decition->leftlamp = false;
  622. gps_decition->rightlamp = false;
  623. /*if(gpsMapLine[PathPoint]->light_left_or_right == 1)
  624. {
  625. gps_decition->leftlamp = true;
  626. gps_decition->rightlamp = false;
  627. }
  628. else if(gpsMapLine[PathPoint]->light_left_or_right == 2)
  629. {
  630. gps_decition->leftlamp = false;
  631. gps_decition->rightlamp = true;
  632. }*/
  633. }else if (gpsMapLine[PathPoint]->roadMode == 1)
  634. {
  635. dSpeed = min(10.0,dSpeed);
  636. }else if (gpsMapLine[PathPoint]->roadMode == 2)
  637. {
  638. dSpeed = min(15.0,dSpeed);
  639. }
  640. else if (gpsMapLine[PathPoint]->roadMode == 7)
  641. {
  642. dSpeed = min(15.0,dSpeed);
  643. xiuzhengCs=1.5;
  644. }else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
  645. {
  646. dSpeed = min(4.0,dSpeed);
  647. }else{
  648. dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
  649. gps_decition->leftlamp = false;
  650. gps_decition->rightlamp = false;
  651. }
  652. if (gpsMapLine[PathPoint]->speed_mode == 2)
  653. {
  654. dSpeed = min(25.0,dSpeed);
  655. }
  656. if((gpsMapLine[PathPoint]->speed)>0.001)
  657. {
  658. dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
  659. }
  660. dSecSpeed = dSpeed / 3.6;
  661. std::cout<<"juecesudu2="<<dSpeed<<std::endl;
  662. }
  663. //std::vector<iv::Perception::PerceptionOutput> lidar_per,
  664. iv::decition::Decition ivdecision_brain::getDecideFromGPS(GPS_INS now_gps_ins,
  665. const std::vector<GPSData> gpsMapLine,
  666. iv::LidarGridPtr lidarGridPtr,
  667. std::vector<iv::Perception::PerceptionOutput> lidar_per,
  668. iv::TrafficLight trafficLight)
  669. {
  670. iv::decition::Decition gps_decition(new iv::decition::DecitionBasic);
  671. //如果useFrenet、useOldAvoid两者不互为相异,则采用原来的方法。“^”为异或运算符。
  672. if(!(useFrenet^useOldAvoid)){
  673. useFrenet = false;
  674. useOldAvoid = true;
  675. }
  676. if (isFirstRun)
  677. {
  678. decision_firstRun(now_gps_ins,gpsMapLine);
  679. }
  680. if(ServiceCarStatus.msysparam.gpsOffset_x!=0 || ServiceCarStatus.msysparam.gpsOffset_y!=0 ){
  681. GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
  682. now_gps_ins.gps_x=gpsOffset.gps_x;
  683. now_gps_ins.gps_y=gpsOffset.gps_y;
  684. GaussProjInvCal(now_gps_ins.gps_x, now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
  685. }
  686. changingDangWei=false;
  687. minDecelerate=0;
  688. if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90){
  689. // int a=0;
  690. gps_decition->wheel_angle = 0;
  691. gps_decition->speed = dSpeed;
  692. gps_decition->accelerator = -0.5;
  693. gps_decition->brake=10;
  694. return gps_decition;
  695. }
  696. if(ServiceCarStatus.daocheMode){
  697. now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
  698. if( now_gps_ins.ins_heading_angle>=360)
  699. now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
  700. }
  701. //1125 traficc
  702. traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
  703. traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
  704. GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
  705. //xiesi
  706. ///kkcai, 2020-01-03
  707. //ServiceCarStatus.busmode=true;
  708. //ServiceCarStatus.busmode=false;//20200102
  709. ///////////////////////////////////////////////////
  710. //busMode = ServiceCarStatus.busmode;
  711. nearStation=false;
  712. gps_decition->leftlamp = false;
  713. gps_decition->rightlamp = false;
  714. // qDebug("heading is %g",now_gps_ins.ins_heading_angle);
  715. aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
  716. aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
  717. aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
  718. GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
  719. is_arrivaled = false;
  720. xiuzhengCs=0;
  721. realspeed = now_gps_ins.speed;
  722. secSpeed = realspeed / 3.6;
  723. if(decision_ParkCalc(now_gps_ins,gps_decition) == 1)
  724. {
  725. return gps_decition;
  726. }
  727. switch (vehState) {
  728. // ChuiZhiTingChe
  729. case reverseCar:
  730. return decision_reverseCar(now_gps_ins);
  731. break;
  732. case reversing:
  733. return decision_reversing(now_gps_ins);
  734. break;
  735. case reverseCircle:
  736. return decision_reverseCircle(now_gps_ins);
  737. break;
  738. case reverseDirect:
  739. return decision_reverseDirect(now_gps_ins);
  740. break;
  741. case reverseArr:
  742. return decision_reverseArr(now_gps_ins);
  743. break;
  744. //ceFangWeiBoChe
  745. case dRever:
  746. return decision_dRever(now_gps_ins);
  747. break;
  748. case dRever0:
  749. return decision_dRever0(now_gps_ins);
  750. break;
  751. case dRever1:
  752. return decision_dRever1(now_gps_ins);
  753. break;
  754. case dRever2:
  755. return decision_dRever2(now_gps_ins);
  756. break;
  757. case dRever3:
  758. return decision_dRever3(now_gps_ins);
  759. break;
  760. case dRever4:
  761. return decision_dRever4(now_gps_ins);
  762. break;
  763. default:
  764. break;
  765. }
  766. obsDistance = -1;
  767. esrIndex = -1;
  768. lidarDistance = -1;
  769. obsDistanceAvoid = -1;
  770. esrIndexAvoid = -1;
  771. roadPre = -1;
  772. gpsTraceOri.clear();
  773. gpsTraceNow.clear();
  774. gpsTraceAim.clear();
  775. gpsTraceAvoid.clear();
  776. gpsTraceBack.clear();
  777. if (!isFirstRun)
  778. {
  779. // PathPoint = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
  780. // if(PathPoint<0){
  781. PathPoint = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
  782. // }
  783. }
  784. if (PathPoint<0)
  785. {
  786. minDecelerate=-1.0;
  787. phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
  788. return gps_decition;
  789. }
  790. lastGpsIndex = PathPoint;
  791. //2020-01-03, kkcai
  792. //if(!circleMode && PathPoint>gpsMapLine.size()-200){
  793. if(!circleMode && PathPoint>gpsMapLine.size()-100){
  794. minDecelerate=-1.0;
  795. std::cout<<"map finish brake"<<std::endl;
  796. }
  797. if(!ServiceCarStatus.inRoadAvoid){
  798. roadOri = gpsMapLine[PathPoint]->roadOri;
  799. roadSum = gpsMapLine[PathPoint]->roadSum;
  800. }else{
  801. roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
  802. roadSum = gpsMapLine[PathPoint]->roadSum*3;
  803. }
  804. if(ServiceCarStatus.avoidObs){
  805. gpsMapLine[PathPoint]->runMode=1;
  806. }else{
  807. gpsMapLine[PathPoint]->runMode=0;
  808. }
  809. if(roadNowStart){
  810. roadNow=roadOri;
  811. roadNowStart=false;
  812. }
  813. // avoidX = (roadOri-roadNow)*roadWidth;
  814. avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  815. if ( gpsMapLine[PathPoint]->runMode == 0 &&(vehState == avoidObs||vehState ==stopObs||vehState == preAvoid
  816. ||vehState == avoiding||vehState == backOri||vehState ==preBack||vehState ==waitAvoid ) )
  817. {
  818. vehState = normalRun;
  819. roadNow = roadOri;
  820. }
  821. gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
  822. // gpsTraceOri = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
  823. if(gpsTraceOri.empty()){
  824. gps_decition->wheel_angle = 0;
  825. gps_decition->speed = dSpeed;
  826. gps_decition->accelerator = -0.5;
  827. gps_decition->brake=10;
  828. return gps_decition;
  829. }
  830. traceDevi=gpsTraceOri[0].x;
  831. //起步进行frenet规划后,车辆行驶至地图路线上,起步操作完毕。
  832. //如果车辆本身启动位置在地图路线上,可不用进行frenet规划。
  833. // if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
  834. // startingFlag = false;
  835. // }
  836. startingFlag = false;
  837. if(startingFlag){
  838. // gpsTraceAim = gpsTraceOri;
  839. if(abs(gpsTraceOri[0].x)>1){
  840. cout<<"frenetPlanner->getPath:pre"<<endl;
  841. gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
  842. cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
  843. if(gpsTraceNow.size()==0){
  844. gps_decition->accelerator = 0;
  845. gps_decition->brake=10;
  846. gps_decition->wheel_angle = lastAngle;
  847. gps_decition->speed = 0;
  848. return gps_decition;
  849. }
  850. }else{
  851. startingFlag = false;
  852. }
  853. }
  854. if(useFrenet){
  855. //如果车辆在变道中,启用frenet规划局部路径
  856. if(vehState == avoiding || vehState == backOri){
  857. //avoidX = (roadOri - roadNow)*roadWidth;
  858. avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  859. gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
  860. //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
  861. gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
  862. if(gpsTraceNow.size()==0){
  863. gps_decition->accelerator = 0;
  864. gps_decition->brake=10;
  865. gps_decition->wheel_angle = lastAngle;
  866. gps_decition->speed = 0;
  867. return gps_decition;
  868. }
  869. }
  870. }
  871. if(gpsTraceNow.size()==0){
  872. if (roadNow==roadOri)
  873. {
  874. gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
  875. //gpsTraceNow = getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex);
  876. }else
  877. {
  878. // gpsTraceNow = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-roadNow))], lastGpsIndex);
  879. gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
  880. //gpsTraceNow = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
  881. }
  882. }
  883. // dSpeed = getSpeed(gpsTraceNow);
  884. dSpeed = 80;
  885. // planTrace = gpsTraceNow; //Add By YuChuli, 2020.11.26
  886. planTrace.clear();
  887. for(int i=0;i<gpsTraceNow.size();i++){
  888. TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
  889. planTrace.push_back(pt);
  890. }
  891. dSpeed = limitSpeed(controlAng, dSpeed);
  892. controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
  893. if(!circleMode && PathPoint>(gpsMapLine.size()-100)){
  894. controlAng=0;
  895. }
  896. //1220
  897. if(ServiceCarStatus.daocheMode){
  898. controlAng=0-controlAng;
  899. }
  900. //2020-0106
  901. if(vehState==avoiding){
  902. controlAng=max(-150.0,controlAng);
  903. controlAng=min(150.0,controlAng);
  904. }
  905. if(vehState==backOri){
  906. controlAng=max(-120.0,controlAng);
  907. controlAng=min(120.0,controlAng);
  908. }
  909. //准备驻车
  910. if (readyZhucheMode)
  911. {
  912. decision_readyZhucheMode(now_gps_ins,gpsMapLine);
  913. }
  914. if (readyParkMode)
  915. {
  916. decision_readyParkMode(now_gps_ins,gpsMapLine);
  917. }
  918. decision_speedctrl(gps_decition,gpsMapLine);
  919. if(vehState==changingRoad){
  920. if(gpsTraceNow[0].x>1.0){
  921. gps_decition->leftlamp = false;
  922. gps_decition->rightlamp = true;
  923. }else if(gpsTraceNow[0].x<-1.0){
  924. gps_decition->leftlamp = true;
  925. gps_decition->rightlamp = false;
  926. }else{
  927. vehState==normalRun;
  928. }
  929. }
  930. // qDebug("decide0 time is %d",xTime.elapsed());
  931. //1220
  932. if(!ServiceCarStatus.daocheMode){
  933. computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
  934. }else{
  935. gpsTraceRear.clear();
  936. for(int i=0;i<gpsTraceNow.size();i++){
  937. Point2D pt;
  938. pt.x=0-gpsTraceNow[i].x;
  939. pt.y=0-gpsTraceNow[i].y;
  940. gpsTraceRear.push_back(pt);
  941. }
  942. computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
  943. // obsDistance=-1; //1227
  944. }
  945. //old_bz--------------------------------------------------------------------------------------------------
  946. if (vehState == avoiding)
  947. {
  948. cout<<"\n==================avoiding=================\n"<<endl;
  949. // vehState=normalRun; //1025
  950. if (dSpeed > 10) {
  951. dSpeed = 10;
  952. }
  953. //若车辆到达变道后的路径,改变车辆状态,关闭frenet规划
  954. if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
  955. vehState = normalRun;
  956. // useFrenet = false;
  957. // useOldAvoid = true;
  958. }
  959. else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
  960. // vehState = avoidObs; 0929
  961. vehState = normalRun;
  962. }
  963. else if (gpsTraceNow[0].x>0)
  964. {
  965. gps_decition->leftlamp = false;
  966. gps_decition->rightlamp = true;
  967. }
  968. else if(gpsTraceNow[0].x<0)
  969. {
  970. gps_decition->leftlamp = true;
  971. gps_decition->rightlamp = false;
  972. }
  973. }
  974. if (vehState==preBack)
  975. {
  976. vehState = backOri;
  977. }
  978. if (vehState==backOri)
  979. {
  980. cout<<"\n================backOri===========\n"<<endl;
  981. // vehState=normalRun; //1025
  982. if (dSpeed > 10) {
  983. dSpeed = 10;
  984. }
  985. //若车辆到达变道后的路径,改变车辆状态,关闭frenet规划
  986. if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
  987. vehState = normalRun;
  988. // useFrenet = false;
  989. // useOldAvoid = true;
  990. }
  991. else if (useOldAvoid && abs(gpsTraceNow[0].x)<1.0) {
  992. // vehState = avoidObs; 0929
  993. vehState = normalRun;
  994. }
  995. else if (gpsTraceNow[0].x>0)
  996. {
  997. gps_decition->leftlamp = false;
  998. gps_decition->rightlamp = true;
  999. }
  1000. else if (gpsTraceNow[0].x<0)
  1001. {
  1002. gps_decition->leftlamp = true;
  1003. gps_decition->rightlamp = false;
  1004. }
  1005. }
  1006. std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
  1007. cout<<"\n当前RoadNum:%d\n"<<roadNow<<endl;
  1008. // 计算回归原始路线
  1009. if (roadNow!=roadOri)
  1010. {
  1011. // useFrenet = true;
  1012. // useOldAvoid = false;
  1013. if(useFrenet){
  1014. if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
  1015. {
  1016. double offsetL = -(roadSum - 1)*roadWidth;
  1017. double offsetR = (roadOri - 0)*roadWidth;
  1018. roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
  1019. }
  1020. }
  1021. else if(useOldAvoid){
  1022. roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
  1023. // avoidX = (roadOri - roadNow)*roadWidth; //1012
  1024. avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  1025. }
  1026. }
  1027. if (roadNow != roadOri && roadPre!=-1)
  1028. {
  1029. roadNow = roadPre;
  1030. // avoidX = (roadOri - roadNow)*roadWidth;
  1031. avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  1032. gpsTraceNow.clear();
  1033. if(useOldAvoid){
  1034. gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
  1035. }
  1036. else if(useFrenet){
  1037. gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
  1038. }
  1039. vehState = backOri;
  1040. hasCheckedBackLidar = false;
  1041. // checkBackObsTimes = 0;
  1042. cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
  1043. }
  1044. //shiyanbizhang 0929
  1045. if(obsDistance>0&& (obsDistance<ServiceCarStatus.socfDis+20.0) &&(abs(realspeed)<1.0) &&
  1046. (vehState==normalRun||vehState==backOri || vehState==avoiding) &&(ObsTimeStart==-1)
  1047. && (gpsMapLine[PathPoint]->runMode==1)){
  1048. ObsTimeStart=GetTickCount();
  1049. cout<<"\n====================preAvoid time count start==================\n"<<endl;
  1050. }
  1051. if(ObsTimeStart!=-1){
  1052. ObsTimeEnd=GetTickCount();
  1053. }
  1054. if(ObsTimeEnd!=-1){
  1055. ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
  1056. }
  1057. if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
  1058. vehState=avoidObs;
  1059. ObsTimeStart=-1;
  1060. ObsTimeEnd=-1;
  1061. ObsTimeSpan=-1;
  1062. cout<<"\n====================preAvoid time count finish==================\n"<<endl;
  1063. }
  1064. if(obsDistance<0 ||obsDistance>ServiceCarStatus.socfDis){
  1065. ObsTimeStart=-1;
  1066. ObsTimeEnd=-1;
  1067. ObsTimeSpan=-1;
  1068. }
  1069. //避障模式
  1070. if (vehState == avoidObs || vehState==waitAvoid ) {
  1071. // if (obsDistance <20 && obsDistance >0)
  1072. if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
  1073. {
  1074. dSpeed = min(7.0,dSpeed);
  1075. avoidTimes++;
  1076. // if (avoidTimes>=6)
  1077. if (avoidTimes>=ServiceCarStatus.aocfTimes)
  1078. {
  1079. vehState = preAvoid;
  1080. avoidTimes = 0;
  1081. }
  1082. }
  1083. // else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
  1084. // {
  1085. // dSpeed = 10;
  1086. // avoidTimes = 0;
  1087. // }
  1088. else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
  1089. {
  1090. dSpeed = min(15.0,dSpeed);
  1091. avoidTimes = 0;
  1092. }
  1093. else
  1094. {
  1095. avoidTimes = 0;
  1096. }
  1097. }
  1098. if (vehState == preAvoid)
  1099. {
  1100. cout<<"\n====================preAvoid==================\n"<<endl;
  1101. /* if (obsDisVector[roadNow]>30)*/ //if (obsDisVector[roadNow]>30||obsDisVector[roadNow]<0)
  1102. if (obsDisVector[roadNow]>ServiceCarStatus.aocfDis)
  1103. {
  1104. // vehState = avoidObs; 0929
  1105. vehState=normalRun;
  1106. }
  1107. else
  1108. {
  1109. //roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine);
  1110. if(useOldAvoid){
  1111. roadPre = chooseAvoidRoad(lidarGridPtr, now_gps_ins,gpsMapLine,lidar_per);
  1112. // avoidX = (roadOri - roadNow)*roadWidth; //1012
  1113. avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  1114. }
  1115. else if(useFrenet){
  1116. double offsetL = -(roadSum - 1)*roadWidth;
  1117. double offsetR = (roadOri - 0)*roadWidth;
  1118. roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
  1119. }
  1120. if (roadPre == -1 && obsDisVector[roadNow]<1.5 && obsDisVector[roadNow]>0)
  1121. {
  1122. // vehState = waitAvoid; 0929
  1123. vehState = avoidObs;
  1124. }
  1125. else if (roadPre != -1)
  1126. {
  1127. if(useOldAvoid){
  1128. roadNow = roadPre;
  1129. // avoidX = (roadOri - roadNow)*roadWidth;
  1130. avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  1131. gpsTraceNow.clear();
  1132. gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
  1133. }
  1134. else if(useFrenet){
  1135. if(roadPre != roadNow){
  1136. avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
  1137. startAvoidGpsInsVector[roadNow] = now_gps_ins;
  1138. }
  1139. roadNow = roadPre;
  1140. // avoidX = (roadOri - roadNow)*roadWidth;
  1141. avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  1142. gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
  1143. }
  1144. vehState = avoiding;
  1145. hasCheckedAvoidLidar = false;
  1146. cout<<"\n避障到RoadNum:%d\n"<<roadNow<<endl;
  1147. }
  1148. }
  1149. }
  1150. //--------------------------------------------------------------------------------old_bz_end
  1151. if (parkMode)
  1152. {
  1153. minDecelerate=-0.4;
  1154. if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
  1155. parkMode=false;
  1156. }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>4){
  1157. parkMode=false;
  1158. }
  1159. }
  1160. //驻车
  1161. if (zhucheMode)
  1162. {
  1163. int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
  1164. // if(trumpet()<16000)
  1165. if (trumpet()<mzcTime)
  1166. {
  1167. // if (trumpet()<12000){
  1168. cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
  1169. minDecelerate=-1.0;
  1170. if(abs(realspeed)<0.2){
  1171. controlAng=0; //tju
  1172. }
  1173. }
  1174. else
  1175. {
  1176. hasZhuched = true; //1125
  1177. zhucheMode = false;
  1178. trumpetFirstCount = true;
  1179. cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
  1180. }
  1181. }
  1182. //判断驻车标志位
  1183. if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
  1184. {
  1185. hasZhuched = false;
  1186. }
  1187. if ( vehState==changingRoad || vehState==chaocheBack)
  1188. {
  1189. double lastAng = 0.0 - lastAngle;
  1190. if (controlAng>40)
  1191. {
  1192. controlAng =40;
  1193. }
  1194. else if (controlAng<-40)
  1195. {
  1196. controlAng = - 40;
  1197. }
  1198. }
  1199. //速度结合角度的限制
  1200. controlAng = limitAngle(realspeed, controlAng);
  1201. std::cout << "\n呼车指令状态:%d\n" << ServiceCarStatus.carState << std::endl;
  1202. //1220
  1203. if(PathPoint+80<gpsMapLine.size()-1){
  1204. if(gpsMapLine[PathPoint+80]->roadMode==4 && !ServiceCarStatus.daocheMode){
  1205. changingDangWei=true;
  1206. }
  1207. }
  1208. if(changingDangWei){
  1209. if(abs(realspeed)>0.1){
  1210. dSpeed=0;
  1211. }else{
  1212. dSpeed=0;
  1213. gps_decition->dangWei=2;
  1214. ServiceCarStatus.daocheMode=true;
  1215. }
  1216. }
  1217. //1220 end
  1218. for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
  1219. {
  1220. GPS_INS gpsIns;
  1221. GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude, &gpsIns.gps_x, &gpsIns.gps_y);
  1222. double dis = GetDistance(gpsIns, now_gps_ins);
  1223. if(dis<20)
  1224. ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
  1225. }
  1226. // 解析云平台数据
  1227. if(ServiceCarStatus.stationCmd.received==true)
  1228. {
  1229. std::vector<Station> station_received;
  1230. Station station_aa,station_nearest;
  1231. if(ServiceCarStatus.stationCmd.has_emergencyStop)
  1232. {
  1233. if(ServiceCarStatus.stationCmd.emergencyStop==0x01)
  1234. {
  1235. //ServiceCarStatus.carState = 0;
  1236. //busMode=true;
  1237. ServiceCarStatus.emergencyStop=1;
  1238. }
  1239. else
  1240. {
  1241. //ServiceCarStatus.carState = 1;
  1242. //busMode=false;
  1243. ServiceCarStatus.emergencyStop=0;
  1244. }
  1245. }
  1246. if(ServiceCarStatus.stationCmd.has_stationStop)
  1247. {
  1248. //寻找最近站点
  1249. station_received.clear();
  1250. for(int i=0;i<ServiceCarStatus.stationCmd.stationTotalNum;i++)
  1251. {
  1252. station_aa.index=i;
  1253. station_aa.station_location.gps_lat=ServiceCarStatus.stationCmd.stationGps[i].gps_lat;
  1254. station_aa.station_location.gps_lng=ServiceCarStatus.stationCmd.stationGps[i].gps_lng;
  1255. 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);
  1256. station_received.push_back(station_aa);
  1257. }
  1258. station_nearest=getNearestStation(now_gps_ins,station_received,gpsMapLine);
  1259. 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);
  1260. givlog->debug("brain_v2x","station_nearest: %d, lat: %.7f, lon: %.7f",
  1261. station_nearest.map_index, station_nearest.station_location.gps_lat,
  1262. station_nearest.station_location.gps_lng);
  1263. //进入站点模式
  1264. if((ServiceCarStatus.stationCmd.stationStop==0x00))
  1265. {
  1266. ServiceCarStatus.carState = 2;
  1267. ServiceCarStatus.amilat=station_nearest.station_location.gps_lat;
  1268. ServiceCarStatus.amilng=station_nearest.station_location.gps_lng;
  1269. busMode=true;
  1270. }else
  1271. {
  1272. ServiceCarStatus.carState = 1;
  1273. busMode=false;
  1274. ServiceCarStatus.station_control_door=0;
  1275. ServiceCarStatus.station_control_door_option=false;
  1276. }
  1277. }
  1278. if(ServiceCarStatus.stationCmd.has_carMode)
  1279. {
  1280. if(ServiceCarStatus.stationCmd.carMode==0x00)
  1281. {
  1282. ServiceCarStatus.stationCmd.mode_manual_drive=true;
  1283. }else
  1284. {
  1285. ServiceCarStatus.stationCmd.mode_manual_drive=false;
  1286. }
  1287. }
  1288. ServiceCarStatus.stationCmd.received=false;
  1289. }
  1290. //carState == 0,紧急停车
  1291. if ((ServiceCarStatus.emergencyStop==1)) //||(adjuseultra()==true))
  1292. {
  1293. minDecelerate=-1.0;
  1294. }
  1295. if (ServiceCarStatus.carState == 3 && busMode)
  1296. {
  1297. if(realspeed<0.1){
  1298. ServiceCarStatus.carState=0;
  1299. minDecelerate=-1.0;
  1300. }else{
  1301. nearStation=true;
  1302. dSpeed=0;
  1303. }
  1304. }
  1305. //carState==2,站点停车
  1306. if ((ServiceCarStatus.carState==2)&&busMode)
  1307. {
  1308. aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
  1309. aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
  1310. GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
  1311. double dis = GetDistance(aim_gps_ins, now_gps_ins);
  1312. Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
  1313. if (dis<20 && pt.y<5 && abs(pt.x)<3)
  1314. {
  1315. dSpeed = 0;
  1316. nearStation=true;
  1317. //is_arrivaled = true;
  1318. //ServiceCarStatus.status[0] = true;
  1319. ServiceCarStatus.istostation=1;
  1320. minDecelerate=-1.0;
  1321. }
  1322. else if (dis<20 && pt.y<15 && abs(pt.x)<3)
  1323. {
  1324. nearStation=true;
  1325. dSpeed = min(8.0, dSpeed);
  1326. }
  1327. else if (dis<30 && pt.y<20 && abs(pt.x)<3)
  1328. {
  1329. dSpeed = min(15.0, dSpeed);
  1330. }
  1331. else if (dis<50 && abs(pt.x)<3)
  1332. {
  1333. dSpeed = min(20.0, dSpeed);
  1334. }
  1335. if((pt.y<5)&&(abs(realspeed)<0.1)&&(ServiceCarStatus.station_control_door==0))
  1336. {
  1337. ServiceCarStatus.station_control_door=1; //open door
  1338. }
  1339. //站点停车log
  1340. givlog->debug("brain_v2x","aim_gps_ins.gps_lat: %.7f, aim_gps_ins.gps_lng: %.7f, dis: %.2f, dSpeed: %.1f",
  1341. aim_gps_ins.gps_lat, aim_gps_ins.gps_lng,
  1342. dis,dSpeed);
  1343. }
  1344. dSecSpeed = dSpeed / 3.6;
  1345. gps_decition->speed = dSpeed;
  1346. if (gpsMapLine[PathPoint]->runMode == 2)
  1347. {
  1348. obsDistance = -1;
  1349. }
  1350. std::cout<<"juecesudu0="<<dSpeed<<std::endl;
  1351. //-------------------------------traffic light----------------------------------------------------------------------------------------
  1352. if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
  1353. traffic_light_pathpoint = iv::decition::Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
  1354. // traffic_light_pathpoint=130;
  1355. float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
  1356. traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
  1357. dSpeed = min((double)traffic_speed,dSpeed);
  1358. if(traffic_speed==0){
  1359. minDecelerate=-2.0;
  1360. }
  1361. }
  1362. //-------------------------------traffic light end-----------------------------------------------------------------------------------------------
  1363. //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
  1364. double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight, gpsMapLine, v2xTrafficVector,
  1365. PathPoint, secSpeed, dSpeed, circleMode);
  1366. dSpeed = min(v2xTrafficSpeed,dSpeed);
  1367. //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
  1368. transferGpsMode2(gpsMapLine);
  1369. if((ServiceCarStatus.msysparam.mvehtype=="hapo")&&(abs(realspeed)<18)){
  1370. if(obsDistance>0 && obsDistance<6){
  1371. dSpeed=0;
  1372. }
  1373. }else if(obsDistance>0 && obsDistance<10){
  1374. dSpeed=0;
  1375. }
  1376. // obsDistance=-1; //1227
  1377. if(ServiceCarStatus.group_control){
  1378. dSpeed = ServiceCarStatus.group_comm_speed;
  1379. }
  1380. if(dSpeed==0){
  1381. minDecelerate=min(-1.0f,minDecelerate);
  1382. }
  1383. std::cout<<"dSpeed= "<<dSpeed<<std::endl;
  1384. // givlog->debug("SPEED","dSpeed is %f",dSpeed);
  1385. gps_decition->wheel_angle = 0.0 - controlAng;
  1386. phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
  1387. lastAngle=gps_decition->wheel_angle;
  1388. // qDebug("decide1 time is %d",xTime.elapsed());
  1389. for(int i=0;i<ServiceCarStatus.obsTraceLeft.size();i++){
  1390. givlog->debug("obs_distance","obsTraceLeftX: %f, obsTraceLeftY: %f",
  1391. ServiceCarStatus.obsTraceLeft.at(i).x, ServiceCarStatus.obsTraceLeft.at(i).y);
  1392. }
  1393. return gps_decition;
  1394. }
  1395. iv::Station ivdecision_brain::getNearestStation(iv::GPS_INS now_gps_ins,std::vector<Station> station_n,std::vector<GPSData> gpsMap){
  1396. int now_index=0,front_index=0;
  1397. int station_size=station_n.size();
  1398. for(int i=0;i<station_size;i++)
  1399. {
  1400. int minDistance=10;
  1401. for (int j = 0; j < gpsMap.size(); j++)
  1402. {
  1403. double tmpdis = GetDistance(station_n[i].station_location, (*gpsMap[j]));
  1404. if (tmpdis < minDistance )
  1405. {
  1406. minDistance = tmpdis;
  1407. station_n[i].map_index=j;
  1408. }
  1409. }
  1410. givlog->debug("brain_v2x","stationi: %d, map_index: %d",
  1411. i,station_n[i].map_index);
  1412. }
  1413. int minDistance=10;
  1414. for (int j = 0; j < gpsMap.size(); j++)
  1415. {
  1416. double tmpdis = GetDistance(now_gps_ins, (*gpsMap[j]));
  1417. if (tmpdis < minDistance )
  1418. {
  1419. minDistance = tmpdis;
  1420. now_index=j;
  1421. }
  1422. }
  1423. givlog->debug("brain_v2x","now_index: %d",
  1424. now_index);
  1425. int min_index=gpsMap.size()-1;
  1426. int station_index=0;
  1427. for(int i=0;i<station_size;i++)
  1428. {
  1429. if(station_n[i].map_index>now_index)
  1430. {
  1431. front_index=station_n[i].map_index;
  1432. if(front_index<min_index)
  1433. {
  1434. min_index=front_index;
  1435. station_index=i;
  1436. }
  1437. }
  1438. }
  1439. qDebug("station_index:%d",station_index);
  1440. return station_n[station_index];
  1441. }
  1442. void ivdecision_brain::initMethods(){
  1443. pid_Controller= new PIDController();
  1444. ge3_Adapter = new Ge3Adapter();
  1445. qingyuan_Adapter = new QingYuanAdapter();
  1446. vv7_Adapter = new VV7Adapter();
  1447. zhongche_Adapter = new ZhongcheAdapter();
  1448. bus_Adapter = new BusAdapter();
  1449. hapo_Adapter = new HapoAdapter();
  1450. yuhesen_Adapter = new YuHeSenAdapter();
  1451. mpid_Controller.reset(pid_Controller);
  1452. mbus_Adapter.reset(bus_Adapter);
  1453. mhapo_Adapter.reset(hapo_Adapter);
  1454. myuhesen_Adapter.reset(yuhesen_Adapter);
  1455. frenetPlanner = new FrenetPlanner();
  1456. // laneChangePlanner = new LaneChangePlanner();
  1457. mfrenetPlanner.reset(frenetPlanner);
  1458. // mlaneChangePlanner.reset(laneChangePlanner);
  1459. }
  1460. void ivdecision_brain::phaseSpeedDecition(iv::decition::Decition decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
  1461. pid_Controller->getControlDecition( gpsIns, gpsTraceNow, dSpeed, obsDis, obsSpeed, false, true, &decition);
  1462. if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
  1463. ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
  1464. }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
  1465. qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
  1466. }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
  1467. vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
  1468. }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
  1469. zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
  1470. }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
  1471. bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
  1472. }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
  1473. hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
  1474. }
  1475. else if(ServiceCarStatus.msysparam.mvehtype=="yuhesen"){
  1476. yuhesen_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed, decition->accelerator,ServiceCarStatus.mfCalAcc ,changingDangWei, &decition);
  1477. }
  1478. }
  1479. std::vector<iv::Point2D> ivdecision_brain::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
  1480. vector<iv::Point2D> trace;
  1481. // int PathPoint = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, lastGpsIndex, minDis, maxAngle);
  1482. /*if (PathPoint != -1)
  1483. lastGpsIndex = PathPoint;*/
  1484. if (gpsMapLine.size() > 600 && PathPoint >= 0) {
  1485. int aimIndex;
  1486. if(circleMode){
  1487. aimIndex=PathPoint+600;
  1488. }else{
  1489. aimIndex=min((PathPoint+600),(int)gpsMapLine.size());
  1490. }
  1491. for (int i = PathPoint; i < aimIndex; i++)
  1492. {
  1493. int index = i % gpsMapLine.size();
  1494. Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
  1495. pt.x += offset_real * 0.032;
  1496. pt.v1 = (*gpsMapLine[index]).speed_mode;
  1497. pt.v2 = (*gpsMapLine[index]).mode2;
  1498. pt.roadMode=(*gpsMapLine[index]).roadMode;
  1499. if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
  1500. {
  1501. readyZhucheMode = true;
  1502. zhuchePointNum = index;
  1503. }
  1504. if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
  1505. {
  1506. readyZhucheMode = true;
  1507. zhuchePointNum = index;
  1508. }
  1509. //csvv7
  1510. if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
  1511. {
  1512. readyParkMode = true;
  1513. finishPointNum = index;
  1514. }
  1515. // if (pt.v1 == 22 || pt.v1 == 23)
  1516. // {
  1517. // readyParkMode = true;
  1518. // finishPointNum = i;
  1519. // }
  1520. switch (pt.v1)
  1521. {
  1522. case 0:
  1523. pt.speed = 80;
  1524. break;
  1525. case 1:
  1526. pt.speed = 25;
  1527. break;
  1528. case 2:
  1529. pt.speed =25;
  1530. break;
  1531. case 3:
  1532. pt.speed = 20;
  1533. break;
  1534. case 4:
  1535. pt.speed =18;
  1536. break;
  1537. case 5:
  1538. pt.speed = 18;
  1539. break;
  1540. case 7:
  1541. pt.speed = 10;
  1542. break;
  1543. case 22:
  1544. pt.speed = 5;
  1545. break;
  1546. case 23:
  1547. pt.speed = 5;
  1548. break;
  1549. default:
  1550. break;
  1551. }
  1552. trace.push_back(pt);
  1553. }
  1554. }
  1555. return trace;
  1556. }
  1557. std::vector<iv::Point2D> ivdecision_brain::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
  1558. if (offset==0)
  1559. {
  1560. return gpsTrace;
  1561. }
  1562. std::vector<iv::Point2D> trace;
  1563. for (int j = 0; j < gpsTrace.size(); j++)
  1564. {
  1565. double sumx1 = 0, sumy1 = 0, count1 = 0;
  1566. double sumx2 = 0, sumy2 = 0, count2 = 0;
  1567. for (int k = max(0, j - 4); k <= j; k++)
  1568. {
  1569. count1 = count1 + 1;
  1570. sumx1 += gpsTrace[k].x;
  1571. sumy1 += gpsTrace[k].y;
  1572. }
  1573. for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
  1574. {
  1575. count2 = count2 + 1;
  1576. sumx2 += gpsTrace[k].x;
  1577. sumy2 += gpsTrace[k].y;
  1578. }
  1579. sumx1 /= count1; sumy1 /= count1;
  1580. sumx2 /= count2; sumy2 /= count2;
  1581. double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
  1582. double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
  1583. double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
  1584. double avoidLenth = abs(offset);
  1585. if (offset<0)
  1586. {
  1587. Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + M_PI / 2),
  1588. carFronty + avoidLenth * sin(anglevalue + M_PI / 2));
  1589. ptLeft.speed = gpsTrace[j].speed;
  1590. ptLeft.v1 = gpsTrace[j].v1;
  1591. ptLeft.v2 = gpsTrace[j].v2;
  1592. trace.push_back(ptLeft);
  1593. }
  1594. else
  1595. {
  1596. Point2D ptRight(carFrontx + avoidLenth * cos(anglevalue - M_PI / 2),
  1597. carFronty + avoidLenth * sin(anglevalue - M_PI / 2));
  1598. ptRight.speed = gpsTrace[j].speed;
  1599. ptRight.v1 = gpsTrace[j].v1;
  1600. ptRight.v2 = gpsTrace[j].v2;
  1601. trace.push_back(ptRight);
  1602. }
  1603. }
  1604. return trace;
  1605. }
  1606. double ivdecision_brain::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
  1607. double angle=0;
  1608. if ( abs(minDis) < 20 && abs(maxAngle) < 100)
  1609. {
  1610. // angle = iv::decition::iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
  1611. pid_Controller->getControlDecition(gpsIns, gpsTrace, -1, -1, -1, true, false, &decition);
  1612. angle= decition->wheel_angle;
  1613. }
  1614. return angle;
  1615. }
  1616. double ivdecision_brain::getSpeed(std::vector<Point2D> gpsTrace) {
  1617. double speed=0;
  1618. int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
  1619. speed = gpsTrace[speedPoint].speed;
  1620. for (int i = 0; i < speedPoint; i++) {
  1621. speed = min(speed, gpsTrace[i].speed);
  1622. }
  1623. return speed;
  1624. }
  1625. //void ivdecision_brain::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
  1626. //
  1627. // if (!obsRadars.empty())
  1628. // {
  1629. // esrIndex = iv::decition::iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
  1630. //
  1631. // if (esrIndex != -1)
  1632. // {
  1633. // esrDistance = obsRadars[esrIndex].nomal_y;
  1634. //
  1635. //
  1636. //
  1637. // obsSpeed = obsRadars[esrIndex].speed_y;
  1638. //
  1639. // }
  1640. // else {
  1641. // esrDistance = -1;
  1642. // }
  1643. //
  1644. // }
  1645. // else
  1646. // {
  1647. // esrIndex = -1;
  1648. // esrDistance = -1;
  1649. // }
  1650. // if (esrDistance < 0) {
  1651. // ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
  1652. // }
  1653. // else {
  1654. // ODS("\n------------------>ESR障碍物距离:%f ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
  1655. // }
  1656. //
  1657. // ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
  1658. //}
  1659. void ivdecision_brain::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
  1660. int esrPathpoint;
  1661. esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint,xiuzhengCs);
  1662. if (esrIndex != -1)
  1663. {
  1664. //优化
  1665. // double distance = 0.0;
  1666. // for(int i=0; i<esrPathpoint; ++i){
  1667. // distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
  1668. // }
  1669. // esrDistance = distance - Esr_Y_Offset;
  1670. // if(esrDistance<=0){
  1671. // esrDistance=1;
  1672. // }
  1673. esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
  1674. obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  1675. }
  1676. else {
  1677. esrDistance = -1;
  1678. }
  1679. }
  1680. void ivdecision_brain::getEsrObsDistanceAvoid() {
  1681. esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
  1682. if (esrIndexAvoid != -1)
  1683. {
  1684. esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
  1685. obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
  1686. }
  1687. else {
  1688. esrDistanceAvoid = -1;
  1689. }
  1690. if (esrDistanceAvoid < 0) {
  1691. std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
  1692. }
  1693. else {
  1694. 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;
  1695. }
  1696. std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
  1697. }
  1698. double ivdecision_brain::limitAngle(double speed, double angle) {
  1699. double preAngle = angle;
  1700. if (speed > 15)
  1701. {
  1702. if (preAngle > 350)
  1703. {
  1704. preAngle = 350;
  1705. }
  1706. if (preAngle < -350)
  1707. {
  1708. preAngle = -350;
  1709. }
  1710. }
  1711. if (speed > 22)
  1712. {
  1713. if (preAngle > 200)
  1714. {
  1715. preAngle = 200;
  1716. }
  1717. if (preAngle < -200)
  1718. {
  1719. preAngle = -200;
  1720. }
  1721. }
  1722. if (speed > 25)
  1723. {
  1724. if (preAngle > 150)
  1725. {
  1726. preAngle = 150;
  1727. }
  1728. if (preAngle < -150)
  1729. {
  1730. preAngle = -150;
  1731. }
  1732. }
  1733. if (speed > 30)
  1734. {
  1735. if (preAngle > 70)
  1736. {
  1737. preAngle = 70;
  1738. }
  1739. if (preAngle < -70)
  1740. {
  1741. preAngle = -70;
  1742. }
  1743. }
  1744. if (speed > 45) //20
  1745. {
  1746. if (preAngle > 15)
  1747. {
  1748. preAngle = 15;
  1749. }
  1750. if (preAngle < -15)
  1751. {
  1752. preAngle = -15;
  1753. }
  1754. }
  1755. return preAngle;
  1756. }
  1757. double ivdecision_brain::limitSpeed(double angle, double speed) {
  1758. if (abs(angle) > 500 && speed > 8) speed = 8;
  1759. else if (abs(angle) > 350 && speed > 14) speed = 14;
  1760. else if (abs(angle) > 200 && speed > 21) speed = 21;
  1761. else if (abs(angle) > 150 && speed > 24) speed = 24;
  1762. else if (abs(angle) > 60 && speed > 29) speed = 29;
  1763. else if (abs(angle) > 20 && speed > 34) speed = 34;
  1764. return max(0.0, speed);
  1765. }
  1766. bool ivdecision_brain::checkAvoidEnable(double avoidX, iv::LidarGridPtr lidarGridPtr,int roadNum) {
  1767. if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
  1768. (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<15))
  1769. {
  1770. return false;
  1771. }
  1772. if (roadNum-roadNow>1)
  1773. {
  1774. for (int i = roadNow+1; i < roadNum; i++)
  1775. {
  1776. if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
  1777. return false;
  1778. }
  1779. }
  1780. }
  1781. else if (roadNow-roadNum>1)
  1782. {
  1783. for (int i = roadNow-1; i >roadNum; i--)
  1784. {
  1785. if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
  1786. return false;
  1787. }
  1788. }
  1789. }
  1790. return true;
  1791. }
  1792. bool ivdecision_brain::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
  1793. //lsn
  1794. if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
  1795. {
  1796. return false;
  1797. }
  1798. if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
  1799. (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
  1800. {
  1801. return false;
  1802. }
  1803. if (roadNum - roadNow>1)
  1804. {
  1805. for (int i = roadNow + 1; i < roadNum; i++)
  1806. {
  1807. if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
  1808. return false;
  1809. }
  1810. }
  1811. }
  1812. else if (roadNow - roadNum>1)
  1813. {
  1814. for (int i = roadNow - 1; i >roadNum; i--)
  1815. {
  1816. if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
  1817. return false;
  1818. }
  1819. }
  1820. }
  1821. return true;
  1822. }
  1823. void ivdecision_brain::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
  1824. if (lidarGridPtr == NULL)
  1825. {
  1826. lidarDistanceAvoid = lastLidarDisAvoid;
  1827. }
  1828. else {
  1829. obsPointAvoid = iv::decition::Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr,lidarDistanceAvoid);
  1830. lastLidarDisAvoid = lidarDistanceAvoid;
  1831. }
  1832. std::cout << "\nLidarAvoid距离:%f\n" << lidarDistanceAvoid << std::endl;
  1833. getEsrObsDistanceAvoid();
  1834. //lidarDistanceAvoid = -1; //20200103 apollo_fu
  1835. if (esrDistanceAvoid>0 && lidarDistanceAvoid > 0)
  1836. {
  1837. if (lidarDistanceAvoid >= esrDistanceAvoid)
  1838. {
  1839. obsDistanceAvoid = esrDistanceAvoid;
  1840. obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
  1841. }
  1842. else if (!ServiceCarStatus.obs_radar.empty())
  1843. {
  1844. obsDistanceAvoid = lidarDistanceAvoid;
  1845. obsSpeedAvoid = iv::decition::Compute00().getObsSpeed(obsPointAvoid, secSpeed);
  1846. std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
  1847. }
  1848. else
  1849. {
  1850. obsDistanceAvoid = lidarDistanceAvoid;
  1851. obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
  1852. std::cout << "\n毫米波无数据,计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
  1853. }
  1854. }
  1855. else if (esrDistanceAvoid>0)
  1856. {
  1857. obsDistanceAvoid = esrDistanceAvoid;
  1858. obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
  1859. }
  1860. else if (lidarDistanceAvoid > 0)
  1861. {
  1862. obsDistanceAvoid = lidarDistanceAvoid;
  1863. obsSpeedAvoid = iv::decition::Compute00().getObsSpeed(obsPointAvoid, secSpeed);
  1864. std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
  1865. }
  1866. else {
  1867. obsDistanceAvoid = esrDistanceAvoid;
  1868. obsSpeedAvoid = 0 - secSpeed;
  1869. }
  1870. std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
  1871. if (obsDistanceAvoid <0 && obsLostTimeAvoid<4)
  1872. {
  1873. obsDistanceAvoid = lastDistanceAvoid;
  1874. obsLostTimeAvoid++;
  1875. }
  1876. else
  1877. {
  1878. obsLostTimeAvoid = 0;
  1879. lastDistanceAvoid = -1;
  1880. }
  1881. if (obsDistanceAvoid>0)
  1882. {
  1883. lastDistanceAvoid = obsDistanceAvoid;
  1884. }
  1885. std::cout << "\nODSAvoid距离:%f\n" << obsDistanceAvoid << std::endl;
  1886. }
  1887. void ivdecision_brain::init() {
  1888. for (int i = 0; i < roadSum; i++)
  1889. {
  1890. lastEsrIdVector.push_back(-1);
  1891. lastEsrCountVector.push_back(0);
  1892. GPS_INS gps_ins;
  1893. gps_ins.gps_x = 0;
  1894. gps_ins.gps_y = 0;
  1895. startAvoidGpsInsVector.push_back(gps_ins);
  1896. avoidMinDistanceVector.push_back(0);
  1897. }
  1898. }
  1899. #include <QTime>
  1900. void ivdecision_brain::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
  1901. const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
  1902. // QTime xTime;
  1903. // xTime.start();
  1904. // qDebug("time 0 is %d ",xTime.elapsed());
  1905. double obs,obsSd;
  1906. if (lidarGridPtr == NULL)
  1907. {
  1908. lidarDistance = lastLidarDis;
  1909. // lidarDistance = lastlidarDistance;
  1910. }
  1911. else {
  1912. obsPoint = iv::decition::Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
  1913. float lidarXiuZheng=0;
  1914. if(!ServiceCarStatus.useMobileEye){
  1915. lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
  1916. }
  1917. lidarDistance = obsPoint.y + lidarXiuZheng; //激光距离推到车头 gjw20191110 lidar
  1918. // lidarDistance=-1;
  1919. if (lidarDistance<0)
  1920. {
  1921. lidarDistance = -1;
  1922. }
  1923. lastLidarDis = lidarDistance;
  1924. }
  1925. // obsPredict start
  1926. if(ServiceCarStatus.useObsPredict){
  1927. float preObsDis=200;
  1928. if(!lidar_per.empty()){
  1929. preObsDis=PredictObsDistance( secSpeed, gpsTrace, lidar_per);
  1930. lastPreObsDistance=preObsDis;
  1931. }else{
  1932. preObsDis=lastPreObsDistance;
  1933. }
  1934. if(preObsDis<lidarDistance || lidarDistance==-1){
  1935. lidarDistance=preObsDis;
  1936. }
  1937. }
  1938. // obsPredict end
  1939. // qDebug("time 1 is %d ",xTime.elapsed());
  1940. // if(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
  1941. // lidarDistance=-1;
  1942. // }
  1943. getEsrObsDistance(gpsTrace, roadNum);
  1944. double mobieye_ttc,mobieye_obsid,mobieye_speed,mobieye_distance;
  1945. double fveh_width = 2.0;
  1946. #ifdef USE_MOBIEYE_OBS
  1947. MobieyeInst.GetObsFromMobieye(&gpsTrace,mobieye_distance,mobieye_speed,mobieye_ttc,mobieye_obsid,fveh_width);
  1948. #endif
  1949. // qDebug("time 2 is %d ",xTime.elapsed());
  1950. // esrDistance=-1;
  1951. // if(PathPoint>2992 && PathPoint<4134){
  1952. // lidarDistance=-1;
  1953. // }
  1954. // if(PathPoint>10193 && PathPoint<10929){
  1955. // esrDistance=-1;
  1956. // }
  1957. if(lidarDistance<0){
  1958. lidarDistance=500;
  1959. }
  1960. #ifdef USE_MOBIEYE_OBS
  1961. esrDistance = mobieye_distance;
  1962. if(esrDistance>150){
  1963. esrDistance=500;
  1964. }
  1965. #else
  1966. if(esrDistance>30){
  1967. esrDistance=500;
  1968. }
  1969. #endif
  1970. if(esrDistance<0){
  1971. esrDistance=500;
  1972. }
  1973. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
  1974. // if(esrDistance>30){
  1975. // esrDistance=-1;
  1976. // }
  1977. // if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
  1978. // if(abs(lidarDistance-esrDistance)>5){
  1979. // esrDistance=-1;
  1980. // }
  1981. // }
  1982. // if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
  1983. // && gpsMapLine[PathPoint]->runMode == 1 ){
  1984. // if(abs(lidarDistance-esrDistance)>5){
  1985. // esrDistance=-1;
  1986. // }
  1987. // }
  1988. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
  1989. myesrDistance = esrDistance;
  1990. if(lidarDistance==500){
  1991. lidarDistance=-1;
  1992. }
  1993. if(esrDistance==500){
  1994. esrDistance=-1;
  1995. }
  1996. ServiceCarStatus.mRadarObs = esrDistance;
  1997. ServiceCarStatus.mLidarObs = lidarDistance;
  1998. //zhuanwan pingbi haomibo
  1999. if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
  2000. esrDistance=-1;
  2001. }
  2002. if (esrDistance>0 && lidarDistance > 0)
  2003. {
  2004. if (lidarDistance >= esrDistance)
  2005. {
  2006. #ifdef USE_MOBIEYE_OBS
  2007. obs = esrDistance;
  2008. obsSd = mobieye_speed;
  2009. #else
  2010. // obsDistance = esrDistance;
  2011. obs = esrDistance;
  2012. // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2013. obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2014. #endif
  2015. }
  2016. else if (!ServiceCarStatus.obs_radar.empty())
  2017. {
  2018. // obsDistance = lidarDistance;
  2019. obs = lidarDistance;
  2020. // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2021. obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2022. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
  2023. }
  2024. else
  2025. {
  2026. // obsDistance = lidarDistance;
  2027. obs=lidarDistance;
  2028. // obsSpeed = 0 - secSpeed;
  2029. obsSd = 0 -secSpeed;
  2030. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据,计算obsSpeed:" << obsSpeed << std::endl;
  2031. }
  2032. }
  2033. else if (esrDistance>0)
  2034. {
  2035. // obsDistance = esrDistance;
  2036. // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2037. #ifdef USE_MOBIEYE_OBS
  2038. obs = esrDistance;
  2039. obsSd = mobieye_speed;
  2040. #else
  2041. obs = esrDistance;
  2042. obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2043. #endif
  2044. }
  2045. else if (lidarDistance > 0)
  2046. {
  2047. // obsDistance = lidarDistance;
  2048. // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2049. obs = lidarDistance;
  2050. obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2051. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
  2052. }
  2053. else {
  2054. // obsDistance = esrDistance;
  2055. // obsSpeed = 0 - secSpeed;
  2056. obs = esrDistance;
  2057. obsSd = 0 - secSpeed;
  2058. }
  2059. if(roadNum==roadNow){
  2060. obsDistance=obs;
  2061. obsSpeed=obsSd;
  2062. }
  2063. // if (obsDistance <0 && obsLostTime<4)
  2064. // {
  2065. // obsDistance = lastDistance;
  2066. // obsLostTime++;
  2067. // }
  2068. // else
  2069. // {
  2070. // obsLostTime = 0;
  2071. // lastDistance = -1;
  2072. // }
  2073. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
  2074. ServiceCarStatus.mObs = obsDistance;
  2075. if(ServiceCarStatus.mObs>100){
  2076. ServiceCarStatus.mObs =-1;
  2077. }
  2078. if (obsDistance>0)
  2079. {
  2080. lastDistance = obsDistance;
  2081. }
  2082. //lsn
  2083. if(obs<0){
  2084. obsDisVector[roadNum]=500;
  2085. }else{
  2086. obsDisVector[roadNum]=obs;
  2087. }
  2088. givlog->debug("obs_distance","obs: %f, lidar_obs: %f, Radar_obs: %f, lidar_obs_x: %f, lidar_obs_y: %f",
  2089. ServiceCarStatus.mObs, ServiceCarStatus.mLidarObs,
  2090. ServiceCarStatus.mRadarObs,obsPoint.x,lidarDistance);
  2091. // qDebug("time 3 is %d ",xTime.elapsed());
  2092. }
  2093. //1220
  2094. void ivdecision_brain::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
  2095. const std::vector<GPSData> gpsMapLine) {
  2096. double obs,obsSd;
  2097. if(!ServiceCarStatus.obs_rear_radar.empty()){
  2098. getRearEsrObsDistance(gpsTrace, roadNum);
  2099. }
  2100. if (lidarGridPtr == NULL)
  2101. {
  2102. lidarDistance = lastLidarDis;
  2103. // lidarDistance = lastlidarDistance;
  2104. }
  2105. else {
  2106. obsPoint = iv::decition::Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
  2107. float lidarXiuZheng=0;
  2108. if(!ServiceCarStatus.useMobileEye){
  2109. lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
  2110. }
  2111. if(abs(obsPoint.y)>lidarXiuZheng)
  2112. lidarDistance = abs(obsPoint.y)-lidarXiuZheng; //激光距离推到车头 1220
  2113. // lidarDistance=-1;
  2114. if (lidarDistance<0)
  2115. {
  2116. lidarDistance = -1;
  2117. }
  2118. lastLidarDis = lidarDistance;
  2119. }
  2120. // if(gpsMapLine[PathPoint]->roadMode==14||gpsMapLine[PathPoint]->roadMode==15){
  2121. // lidarDistance=-1;
  2122. // }
  2123. // getEsrObsDistance(gpsTrace, roadNum);
  2124. esrDistance=-1;
  2125. // if(PathPoint>2992 && PathPoint<4134){
  2126. // lidarDistance=-1;
  2127. // }
  2128. // if(PathPoint>10193 && PathPoint<10929){
  2129. // esrDistance=-1;
  2130. // }
  2131. if(lidarDistance<0){
  2132. lidarDistance=500;
  2133. }
  2134. if(esrDistance<0){
  2135. esrDistance=500;
  2136. }
  2137. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
  2138. // if(esrDistance>30){
  2139. // esrDistance=-1;
  2140. // }
  2141. // if(esrDistance>=15 && esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0 ){
  2142. // if(abs(lidarDistance-esrDistance)>5){
  2143. // esrDistance=-1;
  2144. // }
  2145. // }
  2146. // if(esrDistance>=1&& esrDistance<=50&& ServiceCarStatus.mLidarS>0&&ServiceCarStatus.mLidarPer>0
  2147. // && gpsMapLine[PathPoint]->runMode == 1 ){
  2148. // if(abs(lidarDistance-esrDistance)>5){
  2149. // esrDistance=-1;
  2150. // }
  2151. // }
  2152. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
  2153. myesrDistance = esrDistance;
  2154. if(lidarDistance==500){
  2155. lidarDistance=-1;
  2156. }
  2157. if(esrDistance==500){
  2158. esrDistance=-1;
  2159. }
  2160. ServiceCarStatus.mRadarObs = esrDistance;
  2161. ServiceCarStatus.mLidarObs = lidarDistance;
  2162. //zhuanwan pingbi haomibo
  2163. if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
  2164. esrDistance=-1;
  2165. }
  2166. if (esrDistance>0 && lidarDistance > 0)
  2167. {
  2168. if (lidarDistance >= esrDistance)
  2169. {
  2170. // obsDistance = esrDistance;
  2171. obs = esrDistance;
  2172. // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2173. obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2174. }
  2175. else if (!ServiceCarStatus.obs_radar.empty())
  2176. {
  2177. // obsDistance = lidarDistance;
  2178. obs = lidarDistance;
  2179. // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2180. obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2181. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
  2182. }
  2183. else
  2184. {
  2185. // obsDistance = lidarDistance;
  2186. obs=lidarDistance;
  2187. // obsSpeed = 0 - secSpeed;
  2188. obsSd = 0 -secSpeed;
  2189. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据,计算obsSpeed:" << obsSpeed << std::endl;
  2190. }
  2191. }
  2192. else if (esrDistance>0)
  2193. {
  2194. // obsDistance = esrDistance;
  2195. // obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2196. obs = esrDistance;
  2197. obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2198. }
  2199. else if (lidarDistance > 0)
  2200. {
  2201. // obsDistance = lidarDistance;
  2202. // obsSpeed = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2203. obs = lidarDistance;
  2204. obsSd = iv::decition::Compute00().getObsSpeed(obsPoint, secSpeed);
  2205. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
  2206. }
  2207. else {
  2208. // obsDistance = esrDistance;
  2209. // obsSpeed = 0 - secSpeed;
  2210. obs = esrDistance;
  2211. obsSd = 0 - secSpeed;
  2212. }
  2213. if(roadNum==roadNow){
  2214. obsDistance=obs;
  2215. obsSpeed=obsSd;
  2216. }
  2217. if (obsDistance <0 && obsLostTime<4)
  2218. {
  2219. obsDistance = lastDistance;
  2220. obsLostTime++;
  2221. }
  2222. else
  2223. {
  2224. obsLostTime = 0;
  2225. lastDistance = -1;
  2226. }
  2227. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
  2228. ServiceCarStatus.mObs = obsDistance;
  2229. if(ServiceCarStatus.mObs>100){
  2230. ServiceCarStatus.mObs =-1;
  2231. }
  2232. if (obsDistance>0)
  2233. {
  2234. lastDistance = obsDistance;
  2235. }
  2236. //lsn
  2237. if(obs<0){
  2238. obsDisVector[roadNum]=500;
  2239. }else{
  2240. obsDisVector[roadNum]=obs;
  2241. }
  2242. }
  2243. void ivdecision_brain::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
  2244. double preObsDis;
  2245. if(!lidar_per.empty()){
  2246. preObsDis=PredictObsDistance( realSpeed, gpsTrace, lidar_per);
  2247. lastPreObsDistance=preObsDis;
  2248. }else{
  2249. preObsDis=lastPreObsDistance;
  2250. }
  2251. if(preObsDis<obsDistance){
  2252. obsDistance=preObsDis;
  2253. lastDistance=obsDistance;
  2254. }
  2255. }
  2256. int ivdecision_brain::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
  2257. roadPre = -1;
  2258. // if (roadNow<roadOri)
  2259. // {
  2260. // for (int i = 0; i < roadNow; i++)
  2261. // {
  2262. // gpsTraceAvoid.clear();
  2263. // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
  2264. // avoidX = (roadWidth)*(roadOri - i);
  2265. // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
  2266. // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
  2267. // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
  2268. // }
  2269. // for (int i = roadOri+1; i < roadSum; i++)
  2270. // {
  2271. // gpsTraceAvoid.clear();
  2272. // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
  2273. // avoidX = (roadWidth)*(roadOri - i);
  2274. // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
  2275. // // bsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
  2276. // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
  2277. // }
  2278. // }
  2279. // else if (roadNow>roadOri)
  2280. // {
  2281. // for (int i = 0; i < roadOri; i++)
  2282. // {
  2283. // gpsTraceAvoid.clear();
  2284. // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
  2285. // avoidX = (roadWidth)*(roadOri - i);
  2286. // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
  2287. // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
  2288. // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
  2289. // }
  2290. // for (int i = roadNow + 1; i < roadSum; i++)
  2291. // {
  2292. // gpsTraceAvoid.clear();
  2293. // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
  2294. // avoidX = (roadWidth)*(roadOri - i);
  2295. // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
  2296. // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
  2297. // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
  2298. // }
  2299. // }
  2300. // else
  2301. // {
  2302. // for (int i = 0; i < roadOri; i++)
  2303. // {
  2304. // gpsTraceAvoid.clear();
  2305. // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri - i))], lastGpsIndex);
  2306. // avoidX = (roadWidth)*(roadOri - i);
  2307. // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
  2308. // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
  2309. // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
  2310. // }
  2311. // for (int i = roadOri + 1; i < roadSum; i++)
  2312. // {
  2313. // gpsTraceAvoid.clear();
  2314. // // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
  2315. // avoidX = (roadWidth)*(roadOri - i);
  2316. // gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
  2317. // // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
  2318. // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
  2319. // }
  2320. // }
  2321. for (int i = 0; i < roadSum; i++)
  2322. {
  2323. gpsTraceAvoid.clear();
  2324. // gpsTraceAvoid = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i - roadOri))], lastGpsIndex);
  2325. // avoidX = (roadWidth)*(roadOri - i);
  2326. avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  2327. gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
  2328. // computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, roadNow + i);
  2329. computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
  2330. }
  2331. if (lidarGridPtr!=NULL)
  2332. {
  2333. hasCheckedAvoidLidar = true;
  2334. }
  2335. for(int i=0; i<roadSum;i++){
  2336. std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
  2337. }
  2338. checkAvoidObsTimes++;
  2339. if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
  2340. {
  2341. return - 1;
  2342. }
  2343. for (int i = 1; i < roadSum; i++)
  2344. {
  2345. if (roadNow + i < roadSum) {
  2346. // avoidX = (roadOri-roadNow-i)*roadWidth;
  2347. avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  2348. if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow+i))
  2349. {
  2350. /*if (roadNow==roadOri)
  2351. {
  2352. avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
  2353. startAvoid_gps_ins = now_gps_ins;
  2354. } */
  2355. avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
  2356. startAvoidGpsInsVector[roadNow] = now_gps_ins;
  2357. roadPre = roadNow + i;
  2358. return roadPre;
  2359. }
  2360. }
  2361. if (roadNow - i >= 0)
  2362. {
  2363. // avoidX = (roadOri - roadNow+i)*roadWidth;
  2364. avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  2365. if (checkAvoidEnable(avoidX, lidarGridPtr, roadNow - i))
  2366. {
  2367. /*if (roadNow == roadOri)
  2368. {
  2369. avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
  2370. startAvoid_gps_ins = now_gps_ins;
  2371. }*/
  2372. avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
  2373. startAvoidGpsInsVector[roadNow] = now_gps_ins;
  2374. roadPre = roadNow - i;
  2375. return roadPre;
  2376. }
  2377. }
  2378. }
  2379. return roadPre;
  2380. }
  2381. int ivdecision_brain::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
  2382. roadPre = -1;
  2383. // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
  2384. // if (roadNow>=roadOri+1)
  2385. // {
  2386. // for (int i = roadOri+1; i < roadNow; i++)
  2387. // {
  2388. // gpsTraceBack.clear();
  2389. // // gpsTraceBack = getGpsTrace(now_gps_ins, gmapsL[int(abs(roadWidth * 2)*(i-roadOri))], lastGpsIndex);
  2390. // avoidX = (roadWidth)*(roadOri - i);
  2391. // gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
  2392. // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
  2393. // }
  2394. // }
  2395. // else if (roadNow <= roadOri - 1) {
  2396. // for (int i = roadOri - 1; i > roadNow; i--)
  2397. // {
  2398. // gpsTraceBack.clear();
  2399. // // gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
  2400. // avoidX = (roadWidth)*(roadOri - i);
  2401. // gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
  2402. // computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
  2403. // }
  2404. // }
  2405. for (int i = 0; i <roadSum; i++)
  2406. {
  2407. gpsTraceBack.clear();
  2408. // gpsTraceBack = getGpsTrace(now_gps_ins, gmapsR[int(abs(roadWidth * 2)*(roadOri-i))], lastGpsIndex);
  2409. // avoidX = (roadWidth)*(roadOri - i);
  2410. avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint],ServiceCarStatus.msysparam.vehWidth);
  2411. gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
  2412. computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
  2413. }
  2414. if (lidarGridPtr != NULL)
  2415. {
  2416. hasCheckedBackLidar = true;
  2417. }
  2418. checkBackObsTimes++;
  2419. if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
  2420. {
  2421. return -1;
  2422. }
  2423. //if ((GetDistance(now_gps_ins, startAvoid_gps_ins)>avoidRunDistance)&&
  2424. //(checkReturnEnable(avoidX, lidarGridPtr,roadOri)))
  2425. if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)) &&
  2426. (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
  2427. {
  2428. roadPre = roadOri;
  2429. return roadPre;
  2430. }
  2431. if (roadNow-roadOri>1)
  2432. {
  2433. for (int i = roadOri + 1;i < roadNow;i++) {
  2434. if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
  2435. (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> avoidMinDistanceVector[i])&&
  2436. (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
  2437. {
  2438. roadPre = i;
  2439. return roadPre;
  2440. }
  2441. }
  2442. }
  2443. else if (roadNow <roadOri-1)
  2444. {
  2445. for (int i = roadOri - 1;i > roadNow;i--) {
  2446. if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
  2447. (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
  2448. (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
  2449. {
  2450. roadPre = i;
  2451. return roadPre;
  2452. }
  2453. }
  2454. }
  2455. return roadPre;
  2456. }
  2457. double ivdecision_brain::trumpet() {
  2458. if (trumpetFirstCount)
  2459. {
  2460. trumpetFirstCount = false;
  2461. trumpetLastTime= GetTickCount();
  2462. trumpetTimeSpan = 0.0;
  2463. }
  2464. else
  2465. {
  2466. trumpetStartTime= GetTickCount();
  2467. trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
  2468. trumpetLastTime = trumpetStartTime;
  2469. }
  2470. return trumpetTimeSpan;
  2471. }
  2472. double ivdecision_brain::transferP() {
  2473. if (transferFirstCount)
  2474. {
  2475. transferFirstCount = false;
  2476. transferLastTime= GetTickCount();
  2477. transferTimeSpan = 0.0;
  2478. }
  2479. else
  2480. {
  2481. transferStartTime= GetTickCount();
  2482. transferTimeSpan += transferStartTime - transferLastTime;
  2483. transferLastTime = transferStartTime;
  2484. }
  2485. return transferTimeSpan;
  2486. }
  2487. void ivdecision_brain::handBrakePark(iv::decition::Decition decition, long duringTime, GPS_INS now_gps_ins) {
  2488. if (abs(now_gps_ins.speed)>0.1)
  2489. {
  2490. decition->accelerator = 0;
  2491. decition->brake = 20;
  2492. decition->wheel_angle = 0;
  2493. }
  2494. else
  2495. {
  2496. decition->accelerator = 0;
  2497. decition->brake = 20;
  2498. decition->wheel_angle = 0;
  2499. handPark = true;
  2500. handParkTime = duringTime;
  2501. }
  2502. }
  2503. void ivdecision_brain::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins) {
  2504. gmapsL.clear();
  2505. gmapsR.clear();
  2506. for (int i = 0; i < 31; i++)
  2507. {
  2508. std::vector<iv::GPSData> gpsMapLineBeside;
  2509. // gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
  2510. gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
  2511. gmapsL.push_back(gpsMapLineBeside);
  2512. }
  2513. for (int i = 0; i < 31; i++)
  2514. {
  2515. std::vector<iv::GPSData> gpsMapLineBeside;
  2516. // gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
  2517. gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
  2518. gmapsR.push_back(gpsMapLineBeside);
  2519. }
  2520. }
  2521. bool ivdecision_brain::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr) {
  2522. if (lidarGridPtr == NULL)
  2523. {
  2524. return false;
  2525. // lidarDistance = lastlidarDistance;
  2526. }
  2527. else {
  2528. obsPoint = iv::decition::Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
  2529. double lidarDistance = obsPoint.y - 2.5; //激光距离推到车头
  2530. // ODS("\n超车雷达距离:%f\n", lidarDistance);
  2531. if (lidarDistance >-20 && lidarDistance<35)
  2532. {
  2533. checkChaoCheBackCounts = 0;
  2534. return false;
  2535. }
  2536. else {
  2537. checkChaoCheBackCounts++;
  2538. }
  2539. if (checkChaoCheBackCounts>2) {
  2540. checkChaoCheBackCounts = 0;
  2541. return true;
  2542. }
  2543. }
  2544. return false;
  2545. }
  2546. void ivdecision_brain::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s){
  2547. Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);
  2548. ServiceCarStatus.group_x_local=pt.x;
  2549. // ServiceCarStatus.group_y_local=pt.y;
  2550. ServiceCarStatus.group_y_local=s;
  2551. if(realspeed<0.36){
  2552. ServiceCarStatus.group_velx_local=0;
  2553. ServiceCarStatus.group_vely_local=0;
  2554. }else{
  2555. ServiceCarStatus.group_velx_local=realspeed*sin(theta)/3.6;
  2556. ServiceCarStatus.group_vely_local=realspeed*cos(theta)/3.6;
  2557. }
  2558. ServiceCarStatus.group_pathpoint=PathPoint;
  2559. }
  2560. float ivdecision_brain::ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time, const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
  2561. int pathpoint,float secSpeed,float dSpeed){
  2562. float traffic_speed=200;
  2563. float traffic_dis=0;
  2564. float passTime;
  2565. float passSpeed;
  2566. bool passEnable=false;
  2567. if(abs(secSpeed)<0.1){
  2568. secSpeed=0;
  2569. }
  2570. if(pathpoint <= traffic_light_pathpoint){
  2571. for(int i=pathpoint;i<traffic_light_pathpoint;i++){
  2572. traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
  2573. }
  2574. }else{
  2575. for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
  2576. traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
  2577. }
  2578. for(int i=0;i<traffic_light_pathpoint;i++){
  2579. traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
  2580. }
  2581. }
  2582. // if(traffic_light_color != 0)
  2583. // {
  2584. // int a = 3;
  2585. // }
  2586. if(traffic_light_color==0 && traffic_dis<10){
  2587. traffic_speed=0;
  2588. }
  2589. // else //20200108
  2590. // {
  2591. // traffic_speed=10;
  2592. // }
  2593. return traffic_speed;
  2594. passSpeed = min((float)(dSpeed/3.6),secSpeed);
  2595. passTime = traffic_dis/(dSpeed/3.6);
  2596. switch(traffic_light_color){
  2597. case 0:
  2598. if(passTime>traffic_light_time+1 && traffic_dis>10){
  2599. passEnable=true;
  2600. }else{
  2601. passEnable=false;
  2602. }
  2603. break;
  2604. case 1:
  2605. if(passTime<traffic_light_time-1 && traffic_dis<10){
  2606. passEnable=true;
  2607. }else{
  2608. passEnable = false;
  2609. }
  2610. break;
  2611. case 2:
  2612. if(passTime<traffic_light_time){
  2613. passEnable= true;
  2614. }else{
  2615. passEnable=false;
  2616. }
  2617. break;
  2618. default:
  2619. break;
  2620. }
  2621. if(!passEnable){
  2622. if(traffic_dis<5){
  2623. traffic_speed=0;
  2624. }else if(traffic_dis<10){
  2625. traffic_speed=5;
  2626. }else if(traffic_dis<20){
  2627. traffic_speed=15;
  2628. }else if(traffic_dis<30){
  2629. traffic_speed=25;
  2630. }else if(traffic_dis<50){
  2631. traffic_speed=30;
  2632. }
  2633. }
  2634. return traffic_speed;
  2635. }
  2636. 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)
  2637. {
  2638. // Point2D obsCombinePoint = Point2D(-1,-1);
  2639. iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
  2640. double obsSd;
  2641. if (lidarGridPtr == NULL)
  2642. {
  2643. lidarDistance = lastLidarDis;
  2644. // lidarDistance = lastlidarDistance;
  2645. }
  2646. else {
  2647. obsPoint = iv::decition::Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
  2648. // lidarDistance = obsPoint.y-2.5; //激光距离推到车头
  2649. iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
  2650. lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;
  2651. // lidarDistance=-1;
  2652. if (lidarDistance<0)
  2653. {
  2654. lidarDistance = -1;
  2655. }
  2656. lastLidarDis = lidarDistance;
  2657. }
  2658. FrenetPoint esr_obs_frenet_point;
  2659. getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
  2660. if(lidarDistance<0){
  2661. lidarDistance=500;
  2662. }
  2663. if(esrDistance<0){
  2664. esrDistance=500;
  2665. }
  2666. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
  2667. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波距离:" << esrDistance << std::endl;
  2668. myesrDistance = esrDistance;
  2669. if(lidarDistance==500){
  2670. lidarDistance=-1;
  2671. }
  2672. if(esrDistance==500){
  2673. esrDistance=-1;
  2674. }
  2675. ServiceCarStatus.mRadarObs = esrDistance;
  2676. ServiceCarStatus.mLidarObs = lidarDistance;
  2677. // //zhuanwan pingbi haomibo
  2678. // if(gpsTraceNow[0].v1==4||gpsTraceNow[0].v1==5){
  2679. // esrDistance=-1;
  2680. // }
  2681. if (esrDistance>0 && lidarDistance > 0)
  2682. {
  2683. if (lidarDistance >= esrDistance)
  2684. {
  2685. obs = esrDistance;
  2686. // obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2687. obsSd = obsSpeed;
  2688. //障碍物信息使用毫米波采集的。其位置坐标加上偏移量,转换成相对于惯导的位置,即转换成车辆坐标系下的位置坐标。
  2689. // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
  2690. }
  2691. else if (!ServiceCarStatus.obs_radar.empty())
  2692. {
  2693. obs = lidarDistance;
  2694. // obsCombinePoint = obsPoint;
  2695. // obsSd = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
  2696. obsSd = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
  2697. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
  2698. }
  2699. else
  2700. {
  2701. obs=lidarDistance;
  2702. // obsCombinePoint = obsPoint;
  2703. obsSd = 0 -secSpeed*cos(car_now_frenet_point.tangent_Ang-M_PI/2);
  2704. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据,计算obsSpeed:" << obsSpeed << std::endl;
  2705. }
  2706. }
  2707. else if (esrDistance>0)
  2708. {
  2709. obs = esrDistance;
  2710. // obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
  2711. obsSd = obsSpeed;
  2712. //障碍物信息使用毫米波采集的。其位置坐标加上偏移量,转换成相对于惯导的位置,即转换成车辆坐标系下的位置坐标。
  2713. // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
  2714. }
  2715. else if (lidarDistance > 0)
  2716. {
  2717. obs = lidarDistance;
  2718. // obsCombinePoint = obsPoint;
  2719. obsSd = iv::decition::Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
  2720. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
  2721. }
  2722. else {
  2723. obs = esrDistance;
  2724. // obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
  2725. obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-M_PI/2);
  2726. }
  2727. obsDistance=obs;
  2728. obsSpeed=obsSd;
  2729. std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
  2730. ServiceCarStatus.mObs = obsDistance;
  2731. if(ServiceCarStatus.mObs>100){
  2732. ServiceCarStatus.mObs =-1;
  2733. }
  2734. if (obsDistance>0)
  2735. {
  2736. lastDistance = obsDistance;
  2737. }
  2738. if(obs<0){
  2739. obsDistance=500;
  2740. }else{
  2741. obsDistance=obs;
  2742. }
  2743. }
  2744. void ivdecision_brain::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
  2745. esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum,xiuzhengCs);
  2746. if (esrIndex != -1)
  2747. {
  2748. esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
  2749. obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
  2750. }
  2751. else {
  2752. esrDistance = -1;
  2753. }
  2754. }
  2755. 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) {
  2756. esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps,xiuzhengCs);
  2757. if (esrIndex != -1)
  2758. {
  2759. //障碍物的距离,是障碍物和车辆沿道路上的距离,即s。并不直接是它的x坐标或y坐标或两点直线坐标。
  2760. //严格来说应是 esrDistance=障碍物的s - 车辆的s - Esr_Y_Offset,但这是根据车辆位置实时搜索路径,故车辆的s极其小。
  2761. esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset; //-Esr_Y_Offset是把距离推到车头,换算frenet坐标时,把它加上了的。故需减去。
  2762. double speedx=ServiceCarStatus.obs_radar[esrIndex].speed_x; //障碍物相对于车辆x轴的速度
  2763. double speedy=ServiceCarStatus.obs_radar[esrIndex].speed_y; //障碍物相对于车辆y轴的速度
  2764. double speed_combine = sqrt(speedx*speedx+speedy*speedy); //将x、y轴两个方向的速度求矢量和
  2765. //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
  2766. //所谓道路方向是指,道路上离障碍物的最近的点沿道路弧线的切线方向。
  2767. double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
  2768. obsSpeed = speed_combine*cos(Etheta); //由speed_combine分解的s轴方向上的速度
  2769. }
  2770. else {
  2771. esrDistance = -1;
  2772. }
  2773. }
  2774. void ivdecision_brain::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine){
  2775. v2xTrafficVector.clear();
  2776. for (int var = 0; var < gpsMapLine.size(); var++) {
  2777. if(gpsMapLine[var]->roadMode==6 || gpsMapLine[var]->mode2==1000001){
  2778. v2xTrafficVector.push_back(var);
  2779. }
  2780. }
  2781. }
  2782. float ivdecision_brain::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
  2783. int pathpoint,float secSpeed,float dSpeed, bool circleMode){
  2784. float trafficSpeed=200;
  2785. int nearTraffixPoint=-1;
  2786. float nearTrafficDis=0;
  2787. int traffic_color=0;
  2788. int traffic_time=0;
  2789. bool passThrough=false;
  2790. float dSecSpeed=dSpeed/3.6;
  2791. if(v2xTrafficVector.empty()){
  2792. return trafficSpeed;
  2793. }
  2794. if(!circleMode){
  2795. if(pathpoint>v2xTrafficVector.back()){
  2796. return trafficSpeed;
  2797. }else {
  2798. for(int i=0; i< v2xTrafficVector.size();i++){
  2799. if (pathpoint<= v2xTrafficVector[i]){
  2800. nearTraffixPoint=v2xTrafficVector[i];
  2801. break;
  2802. }
  2803. }
  2804. }
  2805. }else if(circleMode){
  2806. if(pathpoint>v2xTrafficVector.back()){
  2807. nearTraffixPoint=v2xTrafficVector[0];
  2808. }else {
  2809. for(int i=0; i< v2xTrafficVector.size();i++){
  2810. if (pathpoint<= v2xTrafficVector[i]){
  2811. nearTraffixPoint=v2xTrafficVector[i];
  2812. break;
  2813. }
  2814. }
  2815. }
  2816. }
  2817. if(nearTraffixPoint!=-1){
  2818. for(int i=pathpoint;i<nearTraffixPoint;i++){
  2819. nearTrafficDis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
  2820. }
  2821. }
  2822. if(nearTrafficDis>50){
  2823. return trafficSpeed;
  2824. }
  2825. int roadMode = gpsMapLine[pathpoint]->roadMode;
  2826. if(roadMode==14 || roadMode==16){
  2827. traffic_color=trafficLight.leftColor;
  2828. traffic_time=trafficLight.leftTime;
  2829. }else if(roadMode==15 ||roadMode==17){
  2830. traffic_color=trafficLight.rightColor;
  2831. traffic_time=trafficLight.rightTime;
  2832. }else {
  2833. traffic_color=trafficLight.straightColor;
  2834. traffic_time=trafficLight.straightTime;
  2835. }
  2836. passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
  2837. if(passThrough){
  2838. return trafficSpeed;
  2839. }else{
  2840. trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
  2841. if(nearTrafficDis<6){
  2842. float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
  2843. minDecelerate=min(minDecelerate,decelerate);
  2844. }
  2845. return trafficSpeed;
  2846. }
  2847. return trafficSpeed;
  2848. }
  2849. bool ivdecision_brain::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
  2850. float passTime=0;
  2851. if (trafficColor==2 || trafficColor==3){
  2852. return false;
  2853. }else if(trafficColor==0){
  2854. return true;
  2855. }else{
  2856. passTime=trafficDis/dSecSpeed;
  2857. if(passTime+1< trafficTime){
  2858. return true;
  2859. }else{
  2860. return false;
  2861. }
  2862. }
  2863. }
  2864. float ivdecision_brain::computeTrafficSpeedLimt(float trafficDis){
  2865. float limit=200;
  2866. if(trafficDis<10){
  2867. limit = 0;
  2868. }else if(trafficDis<15){
  2869. limit = 5;
  2870. }else if(trafficDis<20){
  2871. limit=10;
  2872. }else if(trafficDis<30){
  2873. limit=15;
  2874. }
  2875. return limit;
  2876. }
  2877. bool ivdecision_brain::adjuseultra(){
  2878. bool front=false,back=false,left=false,right=false;
  2879. for(int i=1;i<=13;i++)
  2880. {
  2881. if((i==2)||(i==3)||(i==4)||(i==5)) //front
  2882. {
  2883. if(ServiceCarStatus.ultraDistance[i]<100)
  2884. {
  2885. front=true;
  2886. }
  2887. }else if((i==1)||(i==12)||(i==6)||(i==7)) //left,right
  2888. {
  2889. if(ServiceCarStatus.ultraDistance[i]<30)
  2890. {
  2891. left=true;
  2892. }
  2893. }else if((i==8)||(i==9)||(i==10)||(i==11)) //back
  2894. {
  2895. if(ServiceCarStatus.ultraDistance[i]<10)
  2896. {
  2897. back=true;
  2898. }
  2899. }
  2900. }
  2901. if(front||left||back)
  2902. return true;
  2903. else
  2904. return false;
  2905. }
  2906. void ivdecision_brain::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
  2907. if( gpsMapLine[PathPoint]->mode2==3000){
  2908. if(obsDistance>5){
  2909. obsDistance=200;
  2910. }
  2911. dSpeed=min(dSpeed,5.0);
  2912. }
  2913. }
  2914. float ivdecision_brain::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){
  2915. if(roadAim==roadOri){
  2916. return 0;
  2917. }
  2918. float x=0;
  2919. float veh_to_roadSide=(gps->mfRoadWidth- ServiceCarStatus.msysparam.vehWidth)*0.5;
  2920. float roadSide_to_roadSide=ServiceCarStatus.msysparam.vehWidth;
  2921. if(!ServiceCarStatus.inRoadAvoid){
  2922. x= (roadOri-roadAim)*gps->mfRoadWidth;
  2923. }else{
  2924. int num=roadOri-roadAim;
  2925. switch (abs(num%3)) {
  2926. case 0:
  2927. x=(num/3)*gps->mfRoadWidth;
  2928. break;
  2929. case 1:
  2930. if(num>0){
  2931. x=(num/3)*gps->mfRoadWidth +veh_to_roadSide;
  2932. }else{
  2933. x=(num/3)*gps->mfRoadWidth -veh_to_roadSide;
  2934. }
  2935. break;
  2936. case 2:
  2937. if(num>0){
  2938. x=(num/3)*gps->mfRoadWidth +veh_to_roadSide+roadSide_to_roadSide;
  2939. }else{
  2940. x=(num/3)*gps->mfRoadWidth -veh_to_roadSide-roadSide_to_roadSide;
  2941. }
  2942. break;
  2943. default:
  2944. break;
  2945. }
  2946. }
  2947. return x;
  2948. }
  2949. }