12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666266726682669267026712672267326742675267626772678267926802681268226832684268526862687268826892690269126922693269426952696269726982699270027012702270327042705270627072708270927102711271227132714271527162717271827192720272127222723272427252726272727282729273027312732273327342735273627372738273927402741274227432744274527462747274827492750275127522753275427552756275727582759276027612762276327642765276627672768276927702771277227732774277527762777277827792780278127822783278427852786278727882789279027912792279327942795279627972798279928002801280228032804280528062807280828092810281128122813281428152816281728182819282028212822282328242825282628272828282928302831283228332834283528362837283828392840284128422843284428452846284728482849285028512852285328542855285628572858285928602861286228632864286528662867286828692870287128722873287428752876287728782879288028812882288328842885288628872888288928902891289228932894289528962897289828992900290129022903290429052906290729082909291029112912291329142915291629172918291929202921292229232924292529262927292829292930293129322933293429352936293729382939294029412942294329442945294629472948294929502951295229532954295529562957295829592960296129622963296429652966296729682969297029712972297329742975297629772978297929802981298229832984298529862987298829892990299129922993299429952996299729982999300030013002300330043005300630073008300930103011301230133014301530163017301830193020302130223023302430253026302730283029303030313032303330343035303630373038303930403041304230433044304530463047304830493050305130523053305430553056305730583059306030613062306330643065306630673068306930703071307230733074307530763077307830793080308130823083308430853086308730883089309030913092309330943095309630973098309931003101310231033104310531063107310831093110311131123113311431153116311731183119312031213122312331243125312631273128312931303131313231333134313531363137313831393140 |
- #include "globalplan.h"
- #include "limits"
- #include <iostream>
- #include <Eigen/Dense>
- #include <Eigen/Cholesky>
- #include <Eigen/LU>
- #include <Eigen/QR>
- #include <Eigen/SVD>
- #include <QDebug>
- #include <QPointF>
- using namespace Eigen;
- extern "C"
- {
- #include "dubins.h"
- }
- static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
- /**
- * @brief GetLineDis 获得点到直线Geometry的距离。
- * @param pline
- * @param x
- * @param y
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
- static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
- double & neary,double & nearhead)
- {
- double fRtn = 1000.0;
- double a1,a2,a3,a4,b1,b2;
- double ratio = pline->GetHdg();
- while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
- while(ratio<0)ratio = ratio+2.0*M_PI;
- double dis1,dis2,dis3;
- double x1,x2,x3,y1,y2,y3;
- x1 = pline->GetX();y1=pline->GetY();
- if((ratio == 0)||(ratio == M_PI))
- {
- a1 = 0;a4=0;
- a2 = 1;b1= pline->GetY();
- a3 = 1;b2= x;
- }
- else
- {
- if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
- {
- a2=0;a3=0;
- a1=1,b1=pline->GetX();
- a4 = 1;b2 = y;
- }
- else
- {
- a1 = tan(ratio) *(-1.0);
- a2 = 1;
- a3 = tan(ratio+M_PI/2.0)*(-1.0);
- a4 = 1;
- b1 = a1*pline->GetX() + a2 * pline->GetY();
- b2 = a3*x+a4*y;
- }
- }
- y2 = y1 + pline->GetLength() * sin(ratio);
- x2 = x1 + pline->GetLength() * cos(ratio);
- Eigen::Matrix2d A;
- A<<a1,a2,
- a3,a4;
- Eigen::Vector2d B(b1,b2);
- Eigen::Vector2d opoint = A.lu().solve(B);
- // std::cout<<opoint<<std::endl;
- x3 = opoint(0);
- y3 = opoint(1);
- dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
- dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
- dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
- // std::cout<<" dis 1 is "<<dis1<<std::endl;
- // std::cout<<" dis 2 is "<<dis2<<std::endl;
- // std::cout<<" dis 3 is "<<dis3<<std::endl;
- if((dis1>pline->GetLength())||(dis2>pline->GetLength())) //Outoff line
- {
- // std::cout<<" out line"<<std::endl;
- if(dis1<dis2)
- {
- fRtn = dis1;
- nearx = x1;neary=y1;nearhead = pline->GetHdg();
- }
- else
- {
- fRtn = dis2;
- nearx = x2;neary=y2;nearhead = pline->GetHdg();
- }
- }
- else
- {
- fRtn = dis3;
- nearx = x3;neary=y3;nearhead = pline->GetHdg();
- }
- // std::cout<<"dis is "<<fRtn<<" x is "<<nearx<<" y is "<<neary<<" head is "<<nearhead<<std::endl;
- return fRtn;
- }
- static int getcirclecrosspoint(QPointF startPos, QPointF endPos, QPointF agvPos, double R,QPointF & point1,QPointF & point2 )
- {
- double m=0;
- double k;
- double b;
- //计算分子
- m=startPos.x()-endPos.x();
- //求直线的方程
- if(0==m)
- {
- k=100000;
- b=startPos.y()-k*startPos.x();
- }
- else
- {
- k=(endPos.y()-startPos.y())/(endPos.x()-startPos.x());
- b=startPos.y()-k*startPos.x();
- }
- // qDebug()<<k<<b;
- double temp = fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b);
- //求直线与圆的交点 前提是圆需要与直线有交点
- if(fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b)<R)
- {
- point1.setX((2*agvPos.x()-2*k*(b-agvPos.y())+sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
- point2.setX((2*agvPos.x()-2*k*(b-agvPos.y())-sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
- point1.setY(k*point1.x()+b);
- point2.setY(k*point2.x()+b);
- return 0;
- }
- return -1;
- }
- /**
- * @brief CalcHdg
- * 计算点0到点1的航向
- * @param p0 Point 0
- * @param p1 Point 1
- **/
- static double CalcHdg(QPointF p0, QPointF p1)
- {
- double x0,y0,x1,y1;
- x0 = p0.x();
- y0 = p0.y();
- x1 = p1.x();
- y1 = p1.y();
- if(x0 == x1)
- {
- if(y0 < y1)
- {
- return M_PI/2.0;
- }
- else
- return M_PI*3.0/2.0;
- }
- double ratio = (y1-y0)/(x1-x0);
- double hdg = atan(ratio);
- if(ratio > 0)
- {
- if(y1 > y0)
- {
- }
- else
- {
- hdg = hdg + M_PI;
- }
- }
- else
- {
- if(y1 > y0)
- {
- hdg = hdg + M_PI;
- }
- else
- {
- hdg = hdg + 2.0*M_PI;
- }
- }
- return hdg;
- }
- static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
- {
- double hdg = CalcHdg(poingarc,point1);
- if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
- else hdg = hdg - M_PI/2.0;
- if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
- if(hdg < 0)hdg = hdg + 2.0*M_PI;
- double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
- double hdgdiff = hdg - parc->GetHdg();
- if(hdgrange >= 0 )
- {
- if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
- }
- else
- {
- if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
- }
- if(fabs(hdgdiff ) < fabs(hdgrange))return true;
- return false;
- }
- static inline double calcpointdis(QPointF p1,QPointF p2)
- {
- return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
- }
- /**
- * @brief GetArcDis
- * 计算点到圆弧的最短距离,首先用点和圆弧中心点的直线与圆的交点,计算点到交点的距离,如果交点在圆弧上,则最短距离是交点之一,否则计算点到圆弧两个端点的距离。
- * @param parc pointer to a arc geomery
- * @param x current x
- * @param y current y
- * @param nearx near x
- * @param neary near y
- * @param nearhead nearhead
- **/
- static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
- double & neary,double & nearhead)
- {
- // double fRtn = 1000.0;
- if(parc->GetCurvature() == 0.0)return 1000.0;
- double R = fabs(1.0/parc->GetCurvature());
- // if(parc->GetX() == 4.8213842690322309e+01)
- // {
- // int a = 1;
- // a = 3;
- // }
- // if(parc->GetCurvature() == -0.0000110203)
- // {
- // int a = 1;
- // a = 3;
- // }
- //calculate arc center
- double x_center,y_center;
- if(parc->GetCurvature() > 0)
- {
- x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
- y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
- }
- else
- {
- x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
- y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
- }
- double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
- QPointF arcpoint;
- arcpoint.setX(x_center);arcpoint.setY(y_center);
- QPointF pointnow;
- pointnow.setX(x);pointnow.setY(y);
- QPointF point1,point2;
- point1.setX(x_center + (R * cos(hdgltoa)));
- point1.setY(y_center + (R * sin(hdgltoa)));
- point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
- point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
- //calculat dis
- bool bp1inarc,bp2inarc;
- bp1inarc =pointinarc(parc,arcpoint,point1);
- bp2inarc =pointinarc(parc,arcpoint,point2);
- double fdis[4];
- fdis[0] = calcpointdis(pointnow,point1);
- fdis[1] = calcpointdis(pointnow,point2);
- fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
- QPointF pointend;
- double hdgrange = parc->GetLength()*parc->GetCurvature();
- double hdgend = parc->GetHdg() + hdgrange;
- while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
- while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
- if(parc->GetCurvature() >0)
- {
- pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
- pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
- }
- else
- {
- pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
- pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
- }
- fdis[3] = calcpointdis(pointnow,pointend);
- int indexmin = -1;
- double fdismin = 1000000.0;
- if(bp1inarc)
- {
- indexmin = 0;fdismin = fdis[0];
- }
- if(bp2inarc)
- {
- if(indexmin == -1)
- {
- indexmin = 1;fdismin = fdis[1];
- }
- else
- {
- if(fdis[1]<fdismin)
- {
- indexmin = 1;fdismin = fdis[1];
- }
- }
- }
- if(indexmin == -1)
- {
- indexmin = 2;fdismin = fdis[2];
- }
- else
- {
- if(fdis[2]<fdismin)
- {
- indexmin = 2;fdismin = fdis[2];
- }
- }
- if(fdis[3]<fdismin)
- {
- indexmin = 3;fdismin = fdis[3];
- }
- switch (indexmin) {
- case 0:
- nearx = point1.x();
- neary = point1.y();
- if(parc->GetCurvature()<0)
- {
- nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
- }
- else
- {
- nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
- }
- break;
- case 1:
- nearx = point2.x();
- neary = point2.y();
- if(parc->GetCurvature()<0)
- {
- nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
- }
- else
- {
- nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
- }
- break;
- case 2:
- nearx = parc->GetX();
- neary = parc->GetY();
- nearhead = parc->GetHdg();
- break;
- case 3:
- nearx = pointend.x();
- neary = pointend.y();
- nearhead = hdgend;
- break;
- default:
- std::cout<<"error in arcdis "<<std::endl;
- break;
- }
- while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
- while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
- return fdismin;
- }
- static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
- double & neary,double & nearhead)
- {
- double x,y,hdg;
- double s = 0.0;
- double fdismin = 100000.0;
- double s0 = pspiral->GetS();
- while(s<pspiral->GetLength())
- {
- pspiral->GetCoords(s0+s,x,y,hdg);
- s = s+0.1;
- double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
- if(fdis<fdismin)
- {
- fdismin = fdis;
- nearhead = hdg;
- nearx = x;
- neary = y;
- }
- }
- return fdismin;
- }
- /**
- * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
- * @param parc
- * @param xnow
- * @param ynow
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
- static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
- double & neary,double & nearhead)
- {
- double s = 0.1;
- double fdismin = 100000.0;
- double xold,yold;
- xold = parc->GetX();
- yold = parc->GetY();
- double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
- if(fdis<fdismin)
- {
- fdismin = fdis;
- nearhead = parc->GetHdg();
- nearx = parc->GetX();
- neary = parc->GetY();
- }
- double s_start = parc->GetS();
- while(s < parc->GetLength())
- {
- double x, y,hdg;//xtem,ytem;
- parc->GetCoords(s_start+s,x,y,hdg);
- if(fdis<fdismin)
- {
- fdismin = fdis;
- nearhead = hdg;
- nearx = x;
- neary = y;
- }
- // xold = x;
- // yold = y;
- s = s+ 0.1;
- }
- return fdismin;
- }
- static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
- double & neary,double & nearhead)
- {
- double x,y,hdg;
- // double s = 0.0;
- double fdismin = 100000.0;
- // double s0 = ppoly->GetS();
- x = ppoly->GetX();
- y = ppoly->GetY();
- double A,B,C,D;
- A = ppoly->GetA();
- B = ppoly->GetB();
- C = ppoly->GetC();
- D = ppoly->GetD();
- const double steplim = 0.3;
- double du = steplim;
- double u = 0;
- double v = 0;
- double oldx,oldy;
- oldx = x;
- oldy = y;
- double xstart,ystart;
- xstart = x;
- ystart = y;
- double hdgstart = ppoly->GetHdg();
- double flen = 0;
- u = u+du;
- while(flen < ppoly->GetLength())
- {
- double fdis = 0;
- v = A + B*u + C*u*u + D*u*u*u;
- x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
- y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
- fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
- if(fdis>(steplim*2.0))du = du/2.0;
- flen = flen + fdis;
- u = u + du;
- hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
- double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
- if(fdisnow<fdismin)
- {
- fdismin = fdisnow;
- nearhead = hdg;
- nearx = x;
- neary = y;
- }
- oldx = x;
- oldy = y;
- }
- return fdismin;
- }
- /**
- * @brief GetNearPoint
- * 计算点到圆弧的最短距离,首先用点和圆弧中心点的直线与圆的交点,计算点到交点的距离,如果交点在圆弧上,则最短距离是交点之一,否则计算点到圆弧两个端点的距离。
- * @param x current x
- * @param y current y
- * @param pxodr OpenDrive
- * @param pObjRoad Road
- * @param pgeo Geometry of road
- * @param nearx near x
- * @param neary near y
- * @param nearhead nearhead
- * @param nearthresh near threshhold
- * @retval if == 0 successfull <0 fail.
- **/
- int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
- double & neary,double & nearhead,const double nearthresh,double &froads)
- {
- double dismin = std::numeric_limits<double>::infinity();
- s = dismin;
- int i;
- double frels;
- for(i=0;i<pxodr->GetRoadCount();i++)
- {
- int j;
- Road * proad = pxodr->GetRoad(i);
- double nx,ny,nh;
- for(j=0;j<proad->GetGeometryBlockCount();j++)
- {
- GeometryBlock * pgb = proad->GetGeometryBlock(j);
- double dis;
- RoadGeometry * pg;
- pg = pgb->GetGeometryAt(0);
- switch (pg->GetGeomType()) {
- case 0: //line
- dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
- // dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
- break;
- case 1:
- dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
- break;
- case 2: //arc
- dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
- break;
- case 3:
- dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
- break;
- case 4:
- dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
- break;
- default:
- dis = 100000.0;
- break;
- }
- if(dis < dismin)
- {
- dismin = dis;
- nearx = nx;
- neary = ny;
- nearhead = nh;
- s = dis;
- froads = frels;
- *pObjRoad = proad;
- *pgeo = pgb;
- }
- // if(pgb->CheckIfLine())
- // {
- // GeometryLine * pline = (GeometryLine *)pgb->GetGeometryAt(0);
- // double dis = GetLineDis(pline,x,y,nx,ny,nh);
- // if(dis < dismin)
- // {
- // dismin = dis;
- // nearx = nx;
- // neary = ny;
- // nearhead = nh;
- // s = dis;
- // *pObjRoad = proad;
- // *pgeo = pgb;
- // }
- // }
- // else
- // {
- // GeometryLine * pline1 = (GeometryLine *)pgb->GetGeometryAt(0);
- // double dis = GetLineDis(pline1,x,y,nx,ny,nh);
- // if(dis < dismin)
- // {
- // dismin = dis;
- // nearx = nx;
- // neary = ny;
- // nearhead = nh;
- // s = dis;
- // *pObjRoad = proad;
- // *pgeo = pgb;
- // }
- // GeometryArc * parc = (GeometryArc *)pgb->GetGeometryAt(1);
- // dis = GetArcDis(parc,x,y,nx,ny,nh);
- // if(dis < dismin)
- // {
- // dismin = dis;
- // nearx = nx;
- // neary = ny;
- // nearhead = nh;
- // s = dis;
- // *pObjRoad = proad;
- // *pgeo = pgb;
- // }
- // pline1 = (GeometryLine *)pgb->GetGeometryAt(2);
- // dis = GetLineDis(pline1,x,y,nx,ny,nh);
- // if(dis < dismin)
- // {
- // dismin = dis;
- // nearx = nx;
- // neary = ny;
- // nearhead = nh;
- // s = dis;
- // *pObjRoad = proad;
- // *pgeo = pgb;
- // }
- // }
- }
- }
- if(s > nearthresh)return -1;
- return 0;
- }
- int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
- const double nearthresh,bool bhdgvalid = true)
- {
- std::cout<<" near thresh "<<nearthresh<<std::endl;
- int i;
- double frels;
- int nminmode = 6;
- for(i=0;i<pxodr->GetRoadCount();i++)
- {
- int j;
- Road * proad = pxodr->GetRoad(i);
- double nx,ny,nh;
- bool bchange = false;
- double localdismin = std::numeric_limits<double>::infinity();
- double localnearx = 0;
- double localneary = 0;
- double localnearhead = 0;
- double locals = 0;
- double localfrels = 0;
- GeometryBlock * pgeolocal;
- localdismin = std::numeric_limits<double>::infinity();
- for(j=0;j<proad->GetGeometryBlockCount();j++)
- {
- GeometryBlock * pgb = proad->GetGeometryBlock(j);
- double dis;
- RoadGeometry * pg;
- pg = pgb->GetGeometryAt(0);
- switch (pg->GetGeomType()) {
- case 0: //line
- dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
- break;
- case 1:
- dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
- break;
- case 2: //arc
- dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
- break;
- case 3:
- dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
- break;
- case 4:
- dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
- break;
- default:
- dis = 100000.0;
- break;
- }
- if(dis<localdismin)
- {
- localdismin = dis;
- localnearx = nx;
- localneary = ny;
- localnearhead = nh;
- locals = dis;
- localfrels = frels;
- pgeolocal = pgb;
- bchange = true;
- }
- }
- std::cout<<" local dismin "<<localdismin<<std::endl;
- if(localdismin < nearthresh)
- {
- iv::nearroad xnear;
- xnear.pObjRoad = proad;
- xnear.pgeob = pgeolocal;
- xnear.nearx = localnearx;
- xnear.neary = localneary;
- xnear.fdis = localdismin;
- xnear.nearhead = localnearhead;
- xnear.frels = localfrels;
- if((xnear.fdis>0)&&(xnear.frels>0.00001)&&(xnear.frels<(proad->GetRoadLength()-0.00001)))
- {
- double fcalhdg = xodrfunc::CalcHdg(QPointF(xnear.nearx,xnear.neary),QPointF(x,y));
- double fhdgdiff = fcalhdg - xnear.nearhead;
- while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
- while(fhdgdiff>= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
- if(fhdgdiff<M_PI)
- {
- double fdisroad = proad->GetRoadLeftWidth(xnear.frels);
- if(fdisroad>xnear.fdis)
- {
- xnear.fdis = 0;
- }
- else
- {
- xnear.fdis = xnear.fdis - fdisroad;
- }
- xnear.lr = 1;
- }
- else
- {
- double fdisroad = proad->GetRoadRightWidth(xnear.frels);
- if(fdisroad>xnear.fdis)
- {
- xnear.fdis = 0;
- }
- else
- {
- xnear.fdis = xnear.fdis - fdisroad;
- }
- xnear.lr = 2;
- }
- }
- else
- {
- if(bhdgvalid == false)
- {
- if(xnear.pObjRoad->GetLaneSectionCount()>0)
- {
- LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
- if(pLS->GetRightLaneCount()>0)
- {
- xnear.lr = 2;
- }
- else
- {
- xnear.lr = 1;
- }
- }
- else
- {
- xnear.lr = 2;
- }
- }
- }
- LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
- if(pLS != NULL)
- {
- if(xnear.fdis < 0.00001)
- {
- if(bhdgvalid == false)
- {
- xnear.nmode = 0;
- }
- else
- {
- double headdiff = hdg - xnear.nearhead;
- while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
- while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
- if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
- {
- xnear.nmode = 0;
- }
- else
- {
- xnear.nmode = 1;
- }
- }
- }
- else
- {
- if(xnear.fdis<5)
- {
- if(bhdgvalid == false)
- {
- xnear.nmode = 2;
- }
- else
- {
- double headdiff = hdg - xnear.nearhead;
- while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
- while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
- if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
- {
- xnear.nmode = 2;
- }
- else
- {
- xnear.nmode = 3;
- }
- }
- }
- else
- {
- if(bhdgvalid == false)
- {
- xnear.nmode = 4;
- }
- else
- {
- double headdiff = hdg - xnear.nearhead;
- while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
- while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
- if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
- {
- xnear.nmode = 4;
- }
- else
- {
- xnear.nmode = 5;
- }
- }
- }
- }
- if(xnear.nmode < nminmode)nminmode = xnear.nmode;
- if(xnear.pObjRoad->GetLaneSectionCount()>0)
- {
- LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
- if((pLS->GetRightLaneCount()>0)&& ((pLS->GetLeftLaneCount()==0)) && (xnear.lr == 1))
- {
- xnear.lr = 2;
- }
- if((pLS->GetRightLaneCount() == 0)&& ((pLS->GetLeftLaneCount()>0)) && (xnear.lr == 2))
- {
- xnear.lr = 1;
- }
- }
- xvectornear.push_back(xnear);
- }
- }
- }
- if(xvectornear.size() == 0)
- {
- std::cout<<" no near. "<<std::endl;
- return -1;
- }
- // std::cout<<" near size: "<<xvectornear.size()<<std::endl;
- if(xvectornear.size() > 1)
- {
- int i;
- for(i=0;i<(int)xvectornear.size();i++)
- {
- if(xvectornear[i].nmode > nminmode)
- {
- xvectornear.erase(xvectornear.begin() + i);
- i = i-1;
- }
- }
- }
- // std::cout<<" after size: "<<xvectornear.size()<<std::endl;
- if((xvectornear.size()>1)&&(nminmode>=4)) //if dis > 5 select small dis
- {
- int i;
- while(xvectornear.size()>1)
- {
- if(xvectornear[1].fdis < xvectornear[0].fdis)
- {
- xvectornear.erase(xvectornear.begin());
- }
- else
- {
- xvectornear.erase(xvectornear.begin()+1);
- }
- }
- }
- return 0;
- }
- /**
- * @brief SelectRoadLeftRight
- * 选择左侧道路或右侧道路
- * 1.选择角度差小于90度的道路一侧,即同侧道路
- * 2.单行路,选择存在的那一侧道路。
- * @param pRoad 道路
- * @param head1 车的航向或目标点的航向
- * @param head_road 目标点的航向
- * @retval 1 left 2 right
- **/
- static int SelectRoadLeftRight(Road * pRoad,const double head1,const double head_road)
- {
- int nrtn = 2;
- double headdiff = head1 - head_road;
- while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
- while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
- bool bSel = false;
- if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0)) //if diff is
- {
- if(pRoad->GetLaneSection(0)->GetLeftLaneCount()>0)
- {
- nrtn = 1;
- bSel = true;
- }
- }
- else
- {
- if(pRoad->GetLaneSection(0)->GetRightLaneCount()>0)
- {
- nrtn = 2;
- bSel = true;
- }
- }
- if(bSel == false)
- {
- if(pRoad->GetLaneSection(0)->GetLeftLaneCount() >0)
- {
- nrtn = 1;
- }
- else
- {
- nrtn = 2;
- }
- }
- return nrtn;
- }
- //static double getlinereldis(GeometryLine * pline,double x,double y)
- //{
- // double x0,y0;
- // x0 = pline->GetX();
- // y0 = pline->GetY();
- // double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
- // if(dis > pline->GetS())
- // {
- // std::cout<<"getlinereldis exceed s S is "<<pline->GetS()<<" dis is "<<dis<<std::endl;
- // return dis;
- // }
- // return dis;
- //}
- //static double getarcreldis(GeometryArc * parc,double x,double y)
- //{
- // double x0,y0;
- // x0 = parc->GetX();
- // y0 = parc->GetY();
- // double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
- // double R = fabs(1.0/parc->GetCurvature());
- // double ang = 2.0* asin(dis/(2.0*R));
- // double frtn = ang * R;
- // return frtn;
- //}
- //static double getparampoly3reldis(GeometryParamPoly3 *parc, double xnow, double ynow)
- //{
- // double s = 0.1;
- // double xold,yold;
- // xold = parc->GetX();
- // yold = parc->GetY();
- // while(s < parc->GetLength())
- // {
- // double x, y,xtem,ytem;
- // xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
- // ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
- // x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
- // y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
- // if(sqrt(pow(xnow - x,2) + pow(ynow -y,2))<0.3)
- // {
- // break;
- // }
- // xold = x;
- // yold = y;
- // s = s+ 0.1;
- // }
- // return s;
- //}
- //static double getreldis(RoadGeometry * prg,double x,double y)
- //{
- // int ngeotype = prg->GetGeomType();
- // double frtn = 0;
- // switch (ngeotype) {
- // case 0:
- // frtn = getlinereldis((GeometryLine *)prg,x,y);
- // break;
- // case 1:
- // break;
- // case 2:
- // frtn = getarcreldis((GeometryArc *)prg,x,y);
- // break;
- // case 3:
- // break;
- // case 4:
- // frtn = getparampoly3reldis((GeometryParamPoly3 *)prg,x,y);
- // break;
- // default:
- // break;
- // }
- // return frtn;
- //}
- //static double getposinroad(Road * pRoad,GeometryBlock * pgeob,double x,double y)
- //{
- // RoadGeometry* prg = pgeob->GetGeometryAt(0);
- // double s1 = prg->GetS();
- // double srtn = s1;
- // return s1 + getreldis(prg,x,y);
- //}
- static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
- {
- std::vector<PlanPoint> xvectorPP;
- int i;
- double s = pline->GetLength();
- int nsize = s/fspace;
- if(nsize ==0)nsize = 1;
- for(i=0;i<nsize;i++)
- {
- double x,y;
- x = pline->GetX() + i *fspace * cos(pline->GetHdg());
- y = pline->GetY() + i *fspace * sin(pline->GetHdg());
- PlanPoint pp;
- pp.x = x;
- pp.y = y;
- pp.dis = i*fspace;
- pp.hdg = pline->GetHdg();
- pp.mS = pline->GetS() + i*fspace;
- xvectorPP.push_back(pp);
- }
- return xvectorPP;
- }
- static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
- {
- std::vector<PlanPoint> xvectorPP;
- // double fRtn = 1000.0;
- if(parc->GetCurvature() == 0.0)return xvectorPP;
- double R = fabs(1.0/parc->GetCurvature());
- //calculate arc center
- double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
- double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
- double arcdiff = fspace/R;
- int nsize = parc->GetLength()/fspace;
- if(nsize == 0)nsize = 1;
- int i;
- for(i=0;i<nsize;i++)
- {
- double x,y;
- PlanPoint pp;
- if(parc->GetCurvature() > 0)
- {
- x = x_center + R * cos(parc->GetHdg() + i*arcdiff - M_PI/2.0);
- y = y_center + R * sin(parc->GetHdg() + i*arcdiff - M_PI/2.0);
- pp.hdg = parc->GetHdg() + i*arcdiff;
- }
- else
- {
- x = x_center + R * cos(parc->GetHdg() -i*arcdiff + M_PI/2.0);
- y = y_center + R * sin(parc->GetHdg() -i*arcdiff + M_PI/2.0);
- pp.hdg = parc->GetHdg() - i*arcdiff;
- }
- pp.x = x;
- pp.y = y;
- pp.dis = i*fspace;
- pp.mS = parc->GetS() + i*fspace;
- xvectorPP.push_back(pp);
- }
- return xvectorPP;
- }
- static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
- {
- double x,y,hdg;
- double s = 0.0;
- double s0 = pspiral->GetS();
- std::vector<PlanPoint> xvectorPP;
- PlanPoint pp;
- while(s<pspiral->GetLength())
- {
- pspiral->GetCoords(s0+s,x,y,hdg);
- pp.x = x;
- pp.y = y;
- pp.hdg = hdg;
- pp.dis = s;
- pp.mS = pspiral->GetS() + s;
- xvectorPP.push_back(pp);
- s = s+fspace;
- }
- return xvectorPP;
- }
- static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
- {
- double x,y;
- x = ppoly->GetX();
- y = ppoly->GetY();
- double A,B,C,D;
- A = ppoly->GetA();
- B = ppoly->GetB();
- C = ppoly->GetC();
- D = ppoly->GetD();
- const double steplim = fspace;
- double du = steplim;
- double u = 0;
- double v = 0;
- double oldx,oldy;
- oldx = x;
- oldy = y;
- double xstart,ystart;
- xstart = x;
- ystart = y;
- double hdgstart = ppoly->GetHdg();
- double flen = 0;
- std::vector<PlanPoint> xvectorPP;
- PlanPoint pp;
- pp.x = xstart;
- pp.y = ystart;
- pp.hdg = hdgstart;
- pp.dis = 0;
- pp.mS = ppoly->GetS();
- xvectorPP.push_back(pp);
- u = du;
- while(flen < ppoly->GetLength())
- {
- double fdis = 0;
- v = A + B*u + C*u*u + D*u*u*u;
- x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
- y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
- fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
- double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
- oldx = x;
- oldy = y;
- if(fdis>(steplim*2.0))du = du/2.0;
- flen = flen + fdis;
- u = u + du;
- pp.x = x;
- pp.y = y;
- pp.hdg = hdg;
- // s = s+ dt;
- pp.dis = flen;;
- pp.mS = ppoly->GetS() + flen;
- xvectorPP.push_back(pp);
- }
- return xvectorPP;
- }
- static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
- {
- double s = 0.1;
- std::vector<PlanPoint> xvectorPP;
- PlanPoint pp;
- double xold,yold;
- xold = parc->GetX();
- yold = parc->GetY();
- double hdg0 = parc->GetHdg();
- pp.x = xold;
- pp.y = yold;
- pp.hdg = hdg0;
- pp.dis = 0;
- // xvectorPP.push_back(pp);
- double dt = 1.0;
- double tcount = parc->GetLength()/0.1;
- if(tcount > 0)
- {
- dt = 1.0/tcount;
- }
- double t;
- s = dt;
- s = 0;
- double ua,ub,uc,ud,va,vb,vc,vd;
- ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
- va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();
- double s_start = parc->GetS();
- while(s < parc->GetLength())
- {
- double x, y,hdg;
- parc->GetCoords(s_start+s,x,y,hdg);
- pp.x = x;
- pp.y = y;
- pp.hdg = hdg;
- pp.dis = pp.dis + fspace;;
- pp.mS = s_start + s;
- xvectorPP.push_back(pp);
- s = s+ fspace;
- }
- return xvectorPP;
- }
- std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
- {
- Road * pRoad = xpath.mpRoad;
- //s_start s_end for select now section data.
- double s_start,s_end;
- LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
- s_start = pLS->GetS();
- if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
- else
- {
- s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
- }
- // if(xpath.mroadid == 10190)
- // {
- // int a = 1;
- // a++;
- // }
- std::vector<PlanPoint> xvectorPPS;
- double s = 0;
- int i;
- for(i=0;i<pRoad->GetGeometryBlockCount();i++)
- {
- GeometryBlock * pgb = pRoad->GetGeometryBlock(i);
- RoadGeometry * prg = pgb->GetGeometryAt(0);
- std::vector<PlanPoint> xvectorPP;
- if(i<(pRoad->GetGeometryBlockCount() -0))
- {
- if(prg->GetS()>s_end)
- {
- continue;
- }
- if((prg->GetS() + prg->GetLength())<s_start)
- {
- continue;
- }
- }
- double s1 = prg->GetLength();
- switch (prg->GetGeomType()) {
- case 0:
- xvectorPP = getlinepoint((GeometryLine *)prg,fspace);
- break;
- case 1:
- xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
- break;
- case 2:
- xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
- break;
- case 3:
- xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
- break;
- case 4:
- xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
- break;
- default:
- break;
- }
- int j;
- if(prg->GetS()<s_start)
- {
- s1 = prg->GetLength() -(s_start - prg->GetS());
- }
- if((prg->GetS() + prg->GetLength())>s_end)
- {
- s1 = s_end - prg->GetS();
- }
- for(j=0;j<xvectorPP.size();j++)
- {
- PlanPoint pp = xvectorPP.at(j);
- // double foffset = pRoad->GetLaneOffsetValue(pp.mS);
- // if(fabs(foffset)>0.001)
- // {
- // pp.x = pp.x + foffset * cos(pp.hdg + M_PI/2.0);
- // pp.y = pp.y + foffset * sin(pp.hdg + M_PI/2.0);
- // }
- pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);
- if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
- {
- if(s_start > prg->GetS())
- {
- pp.dis = s + pp.dis -(s_start - prg->GetS());
- }
- else
- {
- pp.dis = s+ pp.dis;
- }
- pp.nRoadID = atoi(pRoad->GetRoadId().data());
- xvectorPPS.push_back(pp);
- }
- // if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
- // {
- // pp.dis = s + pp.dis;
- // pp.nRoadID = atoi(pRoad->GetRoadId().data());
- // xvectorPPS.push_back(pp);
- // }
- // else
- // {
- // if(prg->GetS()<s_start)
- // {
- // }
- // else
- // {
- // if(pp.dis<(s_end -prg->GetS()))
- // {
- // pp.dis = s + pp.dis;
- // pp.nRoadID = atoi(pRoad->GetRoadId().data());
- // xvectorPPS.push_back(pp);
- // }
- // }
- // }
- }
- s = s+ s1;
- }
- return xvectorPPS;
- }
- std::vector<PlanPoint> GetPoint(Road * pRoad)
- {
- std::vector<PlanPoint> xvectorPPS;
- double s = 0;
- int i;
- for(i=0;i<pRoad->GetGeometryBlockCount();i++)
- {
- GeometryBlock * pgb = pRoad->GetGeometryBlock(i);
- RoadGeometry * prg = pgb->GetGeometryAt(0);
- std::vector<PlanPoint> xvectorPP;
- double s1 = prg->GetLength();
- switch (prg->GetGeomType()) {
- case 0:
- xvectorPP = getlinepoint((GeometryLine *)prg);
- break;
- case 1:
- xvectorPP = getspiralpoint((GeometrySpiral *)prg);
- break;
- case 2:
- xvectorPP = getarcpoint((GeometryArc *)prg);
- break;
- case 3:
- xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
- break;
- case 4:
- xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
- break;
- default:
- break;
- }
- int j;
- for(j=0;j<xvectorPP.size();j++)
- {
- PlanPoint pp = xvectorPP.at(j);
- pp.dis = s + pp.dis;
- pp.nRoadID = atoi(pRoad->GetRoadId().data());
- xvectorPPS.push_back(pp);
- }
- s = s+ s1;
- }
- unsigned int j;
- for(j=0;j<xvectorPPS.size();j++)
- {
- xvectorPPS.at(j).mfCurvature = pRoad->GetRoadCurvature(xvectorPPS.at(j).mS);
- }
- return xvectorPPS;
- }
- int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
- {
- int nrtn = 0;
- int i;
- int dismin = 1000;
- for(i=0;i<xvectorPP.size();i++)
- {
- double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
- if(dis <dismin)
- {
- dismin = dis;
- nrtn = i;
- }
- }
- if(dismin > 10.0)
- {
- std::cout<<"indexinroadpoint near point is error. dis is "<<dismin<<std::endl;
- }
- return nrtn;
- }
- double getwidth(Road * pRoad, int nlane)
- {
- double frtn = 0;
- int i;
- int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- if(nlane == pRoad->GetLaneSection(0)->GetLane(i)->GetId())
- {
- LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
- if(pLW != 0)
- {
- frtn = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0)->GetA();
- break;
- }
- }
- }
- return frtn;
- }
- double getoff(Road * pRoad,int nlane)
- {
- double off = getwidth(pRoad,nlane)/2.0;
- if(nlane < 0)off = off * (-1.0);
- // int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
- int i;
- if(nlane > 0)
- off = off + getwidth(pRoad,0)/2.0;
- else
- off = off - getwidth(pRoad,0)/2.0;
- if(nlane > 0)
- {
- for(i=1;i<nlane;i++)
- {
- off = off + getwidth(pRoad,i);
- }
- }
- else
- {
- for(i=-1;i>nlane;i--)
- {
- off = off - getwidth(pRoad,i);
- }
- }
- // return 0;
- return off;
- }
- namespace iv {
- struct lanewidthabcd
- {
- int nlane;
- double A;
- double B;
- double C;
- double D;
- };
- }
- double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
- {
- if(nlane == 0)return 0;
- double frtn = 0;
- std::vector<double> xvectorlanewidth;
- if(nlane<0)
- xvectorlanewidth = pRoad->GetLaneWidthVector(s,2);
- else
- xvectorlanewidth = pRoad->GetLaneWidthVector(s,1);
- if(abs(nlane)<=xvectorlanewidth.size())
- {
- return xvectorlanewidth[abs(nlane)-1];
- }
- else
- {
- std::cout<<" Warning: getwidth get lane width fault."<<std::endl;
- return 0;
- }
- // int i;
- // int nlanecount = xvectorlwa.size();
- // for(i=0;i<nlanecount;i++)
- // {
- // if(nlane == xvectorlwa.at(i).nlane)
- // {
- // iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
- // frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
- // break;
- // }
- // }
- return frtn;
- }
- std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
- {
- std::vector<iv::lanewidthabcd> xvectorlwa;
- if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
- int i;
- LaneSection * pLS = pRoad->GetLaneSection(0);
- // if(pRoad->GetLaneSectionCount() > 1)
- // {
- // for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
- // {
- // if(s>pRoad->GetLaneSection(i+1)->GetS())
- // {
- // pLS = pRoad->GetLaneSection(i+1);
- // }
- // else
- // {
- // break;
- // }
- // }
- // }
- int nlanecount = pLS->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- iv::lanewidthabcd xlwa;
- LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
- xlwa.nlane = pLS->GetLane(i)->GetId();
- if(pLW != 0)
- {
- xlwa.A = pLW->GetA();
- xlwa.B = pLW->GetB();
- xlwa.C = pLW->GetC();
- xlwa.D = pLW->GetD();
- }
- else
- {
- xlwa.A = 0;
- xlwa.B = 0;
- xlwa.C = 0;
- xlwa.D = 0;
- }
- // if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
- xvectorlwa.push_back(xlwa); //Calculate Road Width, not driving need add in.
- }
- return xvectorlwa;
- }
- inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
- {
- int i;
- double off = 0;
- double a,b,c,d;
- if(xvectorIndex.size() == 0)return off;
- for(i=0;i<(xvectorIndex.size()-1);i++)
- {
- a = xvectorLWA[xvectorIndex[i]].A;
- b = xvectorLWA[xvectorIndex[i]].B;
- c = xvectorLWA[xvectorIndex[i]].C;
- d = xvectorLWA[xvectorIndex[i]].D;
- if(xvectorLWA[xvectorIndex[i]].nlane != 0)
- {
- off = off + a + b*s + c*s*s + d*s*s*s;
- }
- else
- {
- off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
- }
- }
- i = xvectorIndex.size()-1;
- a = xvectorLWA[xvectorIndex[i]].A;
- b = xvectorLWA[xvectorIndex[i]].B;
- c = xvectorLWA[xvectorIndex[i]].C;
- d = xvectorLWA[xvectorIndex[i]].D;
- off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
- if(nlane < 0) off = off*(-1.0);
- // std::cout<<"s is "<<s<<std::endl;
- // std::cout<<" off is "<<off<<std::endl;
- return off;
- }
- double GetRoadWidth(Road * pRoad,int nlane,double s)
- {
- if(nlane<0)return pRoad->GetRightRoadWidth(s);
- return pRoad->GetLeftRoadWidth(s);
- }
- //double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
- //{
- // double fwidth = 0;
- // int i;
- // double a,b,c,d;
- // int nsize = xvectorLWA.size();
- // for(i=0;i<nsize;i++)
- // {
- // if(nlane*(xvectorLWA[i].nlane)>0)
- // {
- // a = xvectorLWA[i].A;
- // b = xvectorLWA[i].B;
- // c = xvectorLWA[i].C;
- // d = xvectorLWA[i].D;
- // fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
- // }
- // }
- // return fwidth;
- //}
- int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
- {
- if(pRoad->GetLaneSectionCount() < 1)return -1;
- LaneSection * pLS = pRoad->GetLaneSection(0);
- int i;
- if(pRoad->GetLaneSectionCount() > 1)
- {
- for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
- {
- if(s>pRoad->GetLaneSection(i+1)->GetS())
- {
- pLS = pRoad->GetLaneSection(i+1);
- }
- else
- {
- break;
- }
- }
- }
- nori = 0;
- ntotal = 0;
- fSpeed = -1; //if -1 no speedlimit for map
- pRoad->GetRoadSpeedMax(s,fSpeed); //Get Road Speed Limit.
- pRoad->GetRoadNoavoid(s,bNoavoid);
- int nlanecount = pLS->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- int nlaneid = pLS->GetLane(i)->GetId();
- if(nlaneid == nlane)
- {
- Lane * pLane = pLS->GetLane(i);
- if(pLane->GetLaneSpeedCount() > 0)
- {
- LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
- int j;
- int nspeedcount = pLane->GetLaneSpeedCount();
- for(j=0;j<(nspeedcount -1);j++)
- {
- if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
- {
- pLSpeed = pLane->GetLaneSpeed(j+1);
- }
- else
- {
- break;
- }
- }
- if(pLSpeed->GetS()<=(s-pLS->GetS()))fSpeed = pLSpeed->GetMax();
- }
- }
- if(nlane<0)
- {
- if(nlaneid < 0)
- {
- if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
- {
- ntotal++;
- if(nlaneid<nlane)nori++;
- }
- }
- }
- else
- {
- if(nlaneid > 0)
- {
- if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
- {
- ntotal++;
- if(nlaneid > nlane)nori++;
- }
- }
- }
- }
- return 0;
- }
- std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
- {
- std::vector<int> xvectorindex;
- int i;
- if(nlane >= 0)
- {
- for(i=0;i<=nlane;i++)
- {
- int j;
- int nsize = xvectorLWA.size();
- for(j=0;j<nsize;j++)
- {
- if(i == xvectorLWA.at(j).nlane )
- {
- xvectorindex.push_back(j);
- break;
- }
- }
- }
- }
- else
- {
- for(i=0;i>=nlane;i--)
- {
- int j;
- int nsize = xvectorLWA.size();
- for(j=0;j<nsize;j++)
- {
- if(i == xvectorLWA.at(j).nlane )
- {
- xvectorindex.push_back(j);
- break;
- }
- }
- }
- }
- return xvectorindex;
- }
- void CalcBorringRoad(pathsection xps,std::vector<PlanPoint> & xvectorPP)
- {
- if(xps.mpRoad->GetRoadBorrowCount() == 0)
- {
- return;
- }
- Road * pRoad = xps.mpRoad;
- unsigned int nborrowsize = pRoad->GetRoadBorrowCount();
- unsigned int i;
- unsigned int nPPCount = xvectorPP.size();
- for(i=0;i<nborrowsize;i++)
- {
- RoadBorrow * pborrow = pRoad->GetRoadBorrow(i);
- if(pborrow == NULL)
- {
- std::cout<<"warning:can't get borrow"<<std::endl;
- continue;
- }
- if((pborrow->GetMode() == "All")||((pborrow->GetMode()=="R2L")&&(xps.mainsel<0))||((pborrow->GetMode()=="L2R")&&(xps.mainsel>0)))
- {
- unsigned int j;
- double soffset = pborrow->GetS();
- double borrowlen = pborrow->GetLength();
- for(j=0;j<xvectorPP.size();j++)
- {
- if((xvectorPP[j].mS>=soffset)&&(xvectorPP[j].mS<=(soffset + borrowlen)))
- {
- xvectorPP[j].mbBoringRoad = true;
- }
- }
- }
- }
- }
- void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const double fvehiclewidth,
- const int nchang1,const int nchang2,const int nchangpoint)
- {
- if(xvectorPP.size()<2)return;
- bool bInlaneavoid = false;
- int i;
- if((xps.mbStartToMainChange == false) && (xps.mbMainToEndChange == false))
- {
- if(xps.mpRoad->GetRoadLength()<30)
- {
- double hdgdiff = xvectorPP.at(xvectorPP.size()-1).hdg - xvectorPP.at(0).hdg;
- if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
- if(hdgdiff>(M_PI/3.0)&&(hdgdiff<(5.0*M_PI/3.0)))
- bInlaneavoid = false;
- else
- bInlaneavoid = true;
- }
- else
- {
- bInlaneavoid = true;
- }
- }
- else
- {
- if(xps.mpRoad->GetRoadLength()>100)bInlaneavoid = true;
- }
- if(bInlaneavoid == false)
- {
- int nvpsize = xvectorPP.size();
- for(i=0;i<nvpsize;i++)
- {
- xvectorPP.at(i).bInlaneAvoid = false;
- xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
- xvectorPP.at(i).my_left = xvectorPP.at(i).y;
- xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
- xvectorPP.at(i).my_right = xvectorPP.at(i).y;
- }
- }
- else
- {
- int nvpsize = xvectorPP.size();
- for(i=0;i<nvpsize;i++)xvectorPP.at(i).bInlaneAvoid = true;
- if((xps.mbStartToMainChange == true)||(xps.mbMainToEndChange == true))
- {
- if(xps.mbStartToMainChange == true)
- {
- for(i=(nchang1 - nchangpoint/2);i<(nchang1 + nchangpoint/2);i++)
- {
- if((i>=0)&&(i<nvpsize))
- xvectorPP.at(i).bInlaneAvoid = false;
- }
- }
- if(xps.mbMainToEndChange)
- {
- for(i=(nchang2 - nchangpoint/2);i<(nchang2 + nchangpoint/2);i++)
- {
- if((i>=0)&&(i<nvpsize))
- xvectorPP.at(i).bInlaneAvoid = false;
- }
- }
- }
- for(i=0;i<nvpsize;i++)
- {
- if(xvectorPP.at(i).bInlaneAvoid)
- {
- double foff = xvectorPP.at(i).mfDisToLaneLeft -0.3-fvehiclewidth*0.5; //30cm + 0.5vehcilewidth
- if(foff < 0.3)
- {
- xvectorPP.at(i).bInlaneAvoid = false;
- xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
- xvectorPP.at(i).my_left = xvectorPP.at(i).y;
- xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
- xvectorPP.at(i).my_right = xvectorPP.at(i).y;
- }
- else
- {
- xvectorPP.at(i).mx_left = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg + M_PI/2.0);
- xvectorPP.at(i).my_left = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg + M_PI/2.0);
- xvectorPP.at(i).mx_right = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg - M_PI/2.0);
- xvectorPP.at(i).my_right = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg - M_PI/2.0);
- }
- }
- }
- }
- }
- double getoff(Road * pRoad,int nlane,const double s)
- {
- int i;
- int nLSCount = pRoad->GetLaneSectionCount();
- double s_section = 0;
- double foff = 0;
- for(i=0;i<nLSCount;i++)
- {
- LaneSection * pLS = pRoad->GetLaneSection(i);
- if(i<(nLSCount -1))
- {
- if(pRoad->GetLaneSection(i+1)->GetS()<s)
- {
- continue;
- }
- }
- s_section = pLS->GetS();
- int nlanecount = pLS->GetLaneCount();
- int j;
- for(j=0;j<nlanecount;j++)
- {
- Lane * pLane = pLS->GetLane(j);
- int k;
- double s_lane = s-s_section;
- if(pLane->GetId() != 0)
- {
- for(k=0;k<pLane->GetLaneWidthCount();k++)
- {
- if(k<(pLane->GetLaneWidthCount()-1))
- {
- if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
- {
- continue;
- }
- }
- s_lane = pLane->GetLaneWidth(k)->GetS();
- break;
- }
- LaneWidth * pLW = pLane->GetLaneWidth(k);
- if(pLW == 0)
- {
- std::cout<<"not find LaneWidth"<<std::endl;
- break;
- }
- double fds = s - s_lane - s_section;
- double fwidth= pLW->GetA() + pLW->GetB() * fds
- +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
- // if((pRoad->GetRoadId() == "211210") &&(nlane == -1) &&(pLane->GetId() == -1))
- // {
- // qDebug("fs is %f width is %f",fds,fwidth);
- // }
- if(nlane == pLane->GetId())
- {
- foff = foff + fwidth/2.0;
- }
- if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
- {
- foff = foff + fwidth;
- }
- }
- }
- break;
- }
- if(pRoad->GetLaneOffsetCount()>0)
- {
- int nLOCount = pRoad->GetLaneOffsetCount();
- int isel = -1;
- for(i=0;i<(nLOCount-1);i++)
- {
- if(pRoad->GetLaneOffset(i+1)->GetS()>s)
- {
- isel = i;
- break;
- }
- }
- if(isel < 0)isel = nLOCount-1;
- LaneOffset * pLO = pRoad->GetLaneOffset(isel);
- double s_off = s - pLO->GetS();
- double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
- +pLO->Getd() * s_off * s_off * s_off;
- foff = foff - off1;
- }
- if(nlane<0)foff = foff*(-1);
- return foff;
- }
- std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
- {
- std::vector<PlanPoint> xvectorPP;
- std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(xps.mpRoad);
- std::vector<int> xvectorindex1,xvectorindex2,xvectorindex3;
- int nchange1,nchange2;
- int nlane1,nlane2,nlane3;
- if(xps.mnStartLaneSel == xps.mnEndLaneSel)
- {
- xps.mainsel = xps.mnStartLaneSel;
- xps.mbStartToMainChange = false;
- xps.mbMainToEndChange = false;
- }
- else
- {
- if(xps.mpRoad->GetRoadLength() < 100)
- {
- xps.mainsel = xps.mnEndLaneSel;
- xps.mbStartToMainChange = true;
- xps.mbMainToEndChange = false;
- }
- }
- double off1,off2,off3;
- if(xps.mnStartLaneSel < 0)
- {
- off1 = getoff(xps.mpRoad,xps.mnStartLaneSel);
- off2 = getoff(xps.mpRoad,xps.mainsel);
- off3 = getoff(xps.mpRoad,xps.mnEndLaneSel);
- xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
- xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
- xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
- nlane1 = xps.mnStartLaneSel;
- nlane2 = xps.mainsel;
- nlane3 = xps.mnEndLaneSel;
- }
- else
- {
- off3 = getoff(xps.mpRoad,xps.mnStartLaneSel);
- off2 = getoff(xps.mpRoad,xps.mainsel);
- off1 = getoff(xps.mpRoad,xps.mnEndLaneSel);
- xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
- xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
- xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
- nlane3 = xps.mnStartLaneSel;
- nlane2 = xps.mainsel;
- nlane1 = xps.mnEndLaneSel;
- }
- int nchangepoint = 300;
- if((nchangepoint * 3) > xvPP.size())
- {
- nchangepoint = xvPP.size()/3;
- }
- int nsize = xvPP.size();
- nchange1 = nsize/3;
- if(nchange1>500)nchange1 = 500;
- nchange2 = nsize*2/3;
- if( (nsize-nchange2)>500)nchange2 = nsize-500;
- // if(nsize < 1000)
- // {
- // std::cout<<"GetLanePoint Error. road id is "<<xps.mpRoad->GetRoadId()<<std::endl;
- // return xvectorPP;
- // }
- double x,y;
- int i;
- if(xps.mainsel < 0)
- {
- for(i=0;i<nsize;i++)
- {
- PlanPoint pp = xvPP.at(i);
- // off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
- off1 = getoff(xps.mpRoad,nlane2,pp.mS);
- pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
- pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
- pp.nlrchange = 0;
- if(xps.mainsel != xps.secondsel)
- {
- off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
- pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
- pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
- if(xps.mainsel >xps.secondsel)
- {
- pp.nlrchange = 1;
- }
- else
- {
- pp.nlrchange = 2;
- }
- }
- else
- {
- pp.mfSecx = pp.x ;
- pp.mfSecy = pp.y ;
- }
- pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
- pp.mfDisToLaneLeft = pp.mWidth/2.0;
- pp.lanmp = 0;
- pp.mfDisToRoadLeft = off1*(-1);
- pp.mfRoadWidth = GetRoadWidth(xps.mpRoad,xps.mainsel,pp.mS);
- GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
- xvectorPP.push_back(pp);
- }
- }
- else
- {
- for(i=0;i<nsize;i++)
- {
- PlanPoint pp = xvPP.at(i);
- // off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
- off1 = getoff(xps.mpRoad,nlane2,pp.mS);
- pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
- pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
- pp.nlrchange = 0;
- if(xps.mainsel != xps.secondsel)
- {
- off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
- pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
- pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
- if(xps.mainsel >xps.secondsel)
- {
- pp.nlrchange = 2;
- }
- else
- {
- pp.nlrchange = 1;
- }
- }
- else
- {
- pp.mfSecx = pp.x ;
- pp.mfSecy = pp.y ;
- }
- pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
- pp.mfDisToLaneLeft = pp.mWidth/2.0;
- pp.lanmp = 0;
- pp.mfDisToRoadLeft = off1;
- pp.mfRoadWidth = GetRoadWidth(xps.mpRoad,xps.mainsel,pp.mS);
- GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
- pp.hdg = pp.hdg + M_PI;
- xvectorPP.push_back(pp);
- }
- // for(i=0;i<(nchange1- nchangepoint/2);i++)
- // {
- // PlanPoint pp = xvPP.at(i);
- // off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
- // pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
- // pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
- // pp.hdg = pp.hdg + M_PI;
- // pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
- // pp.mfDisToLaneLeft = pp.mWidth/2.0;
- // pp.lanmp = 0;
- // pp.mfDisToRoadLeft = off1;
- // pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
- // GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // xvectorPP.push_back(pp);
- // }
- // for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
- // {
- // PlanPoint pp = xvPP.at(i);
- // off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
- // off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
- // double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
- // pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
- // pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
- // pp.hdg = pp.hdg + M_PI;
- // double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
- // pp.mfDisToRoadLeft = offx;
- // pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
- // if(nlane1 == nlane2)
- // {
- // pp.mWidth = flanewidth1;
- // pp.mfDisToLaneLeft = pp.mWidth/2.0;
- // pp.lanmp = 0;
- // GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // }
- // else
- // {
- // if(nlane1 < nlane2)
- // {
- // pp.lanmp = -1;
- // }
- // else
- // {
- // pp.lanmp = 1;
- // }
- // if(i<nchange1)
- // {
- // pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
- // double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
- // if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
- // else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
- // GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // }
- // else
- // {
- // pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
- // double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
- // if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
- // else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
- // GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // }
- // }
- // xvectorPP.push_back(pp);
- // }
- // for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
- // {
- // PlanPoint pp = xvPP.at(i);
- // off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
- // pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
- // pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
- // pp.hdg = pp.hdg + M_PI;
- // pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
- // pp.mfDisToLaneLeft = pp.mWidth/2.0;
- // pp.lanmp = 0;
- // pp.mfDisToRoadLeft = off2;
- // pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
- // GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // xvectorPP.push_back(pp);
- // }
- // for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
- // {
- // PlanPoint pp = xvPP.at(i);
- // off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
- // off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
- // double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
- // pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
- // pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
- // pp.hdg = pp.hdg + M_PI;
- // double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
- // pp.mfDisToRoadLeft = offx;
- // pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
- // if(nlane2 == nlane3)
- // {
- // pp.mWidth = flanewidth1;
- // pp.mfDisToLaneLeft = pp.mWidth/2.0;
- // pp.lanmp = 0;
- // GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // }
- // else
- // {
- // if(nlane2 < nlane3)
- // {
- // pp.lanmp = -1;
- // }
- // else
- // {
- // pp.lanmp = 1;
- // }
- // if(i<nchange2)
- // {
- // pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
- // double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
- // if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
- // else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
- // GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // }
- // else
- // {
- // pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
- // double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
- // if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
- // else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
- // GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // }
- // }
- // xvectorPP.push_back(pp);
- // }
- // for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
- // {
- // PlanPoint pp = xvPP.at(i);
- // off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
- // pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
- // pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
- // pp.hdg = pp.hdg + M_PI;
- // pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
- // pp.mfDisToLaneLeft = pp.mWidth/2.0;
- // pp.lanmp = 0;
- // pp.mfDisToRoadLeft = off3;
- // pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
- // GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
- // xvectorPP.push_back(pp);
- // }
- }
- CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,nchange1,nchange2,nchangepoint);
- if(xps.mpRoad->GetRoadBorrowCount()>0)
- {
- CalcBorringRoad(xps,xvectorPP);
- }
- if(xps.mnStartLaneSel > 0)
- {
- std::vector<PlanPoint> xvvPP;
- int nvsize = xvectorPP.size();
- for(i=0;i<nvsize;i++)
- {
- xvvPP.push_back(xvectorPP.at(nvsize-1-i));
- }
- AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
- return xvvPP;
- }
- // pRoad->GetLaneSection(xps.msectionid)
- AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
- return xvectorPP;
- }
- static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
- {
- if(pRoad->GetSignalCount() == 0)return;
- int nsigcount = pRoad->GetSignalCount();
- int i;
- for(i=0;i<nsigcount;i++)
- {
- Signal * pSig = pRoad->GetSignal(i);
- int nfromlane = -100;
- int ntolane = 100;
- if(pSig->GetlaneValidityCount()>0)
- {
- bool bValid = false;
- unsigned int j;
- std::vector<signal_laneValidity> * pvectorlanevalid = pSig->GetlaneValidityVector();
- unsigned int nsize = static_cast<unsigned int>(pvectorlanevalid->size());
- for(j=0;j<nsize;j++)
- {
- signal_laneValidity * pSig_laneValidity = &pvectorlanevalid->at(j);
- nfromlane = pSig_laneValidity->GetfromLane();
- ntolane = pSig_laneValidity->GettoLane();
- if((nlane >= nfromlane)&&(nlane<=ntolane))
- {
- bValid = true;
- break;
- }
- }
- if(bValid == false)continue;
- }
- // signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
- // if(pSig_laneValidity != 0)
- // {
- // nfromlane = pSig_laneValidity->GetfromLane();
- // ntolane = pSig_laneValidity->GettoLane();
- // }
- // if((nlane < 0)&&(nfromlane >= 0))
- // {
- // continue;
- // }
- // if((nlane > 0)&&(ntolane<=0))
- // {
- // continue;
- // }
- double s = pSig->Gets();
- double fpos = s/pRoad->GetRoadLength();
- if(nlane > 0)fpos = 1.0 -fpos;
- int npos = fpos *xvectorPP.size();
- if(npos <0)npos = 0;
- if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
- while(xvectorPP.at(npos).nSignal != -1)
- {
- if(npos > 0)npos--;
- else break;
- }
- while(xvectorPP.at(npos).nSignal != -1)
- {
- if(npos < (xvectorPP.size()-1))npos++;
- else break;
- }
- xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());
- }
- }
- static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
- Road * pRoad_obj,GeometryBlock * pgeob_obj,
- const double x_now,const double y_now,const double head,
- double nearx,double neary, double nearhead,
- double nearx_obj,double neary_obj,const double fvehiclewidth,const double flen = 1000)
- {
- std::vector<PlanPoint> xvectorPPs;
- double fspace = 0.1;
- if(flen<2000)fspace = 0.1;
- else
- {
- if(flen<5000)fspace = 0.3;
- else
- {
- if(flen<10000)fspace = 0.5;
- else
- fspace = 1.0;
- }
- }
- int i;
- // std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
- std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);
- int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
- std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);
- if(xpathsection[0].mainsel < 0)
- {
- for(i=indexstart;i<xvPP.size();i++)
- {
- xvectorPPs.push_back(xvPP.at(i));
- }
- }
- else
- {
- for(i=(xvPP.size() -indexstart);i<xvPP.size();i++)
- {
- PlanPoint pp;
- pp = xvPP.at(i);
- // pp.hdg = pp.hdg +M_PI;
- xvectorPPs.push_back(pp);
- }
- }
- int npathlast = xpathsection.size() - 1;
- // while(npathlast >= 0)
- // {
- // if(npathlast == 0)break;
- // if(xpathsection[npathlast].mpRoad != xpathsection[npathlast - 1].mpRoad)break;
- // }
- for(i=1;i<(npathlast);i++)
- {
- // if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
- // {
- // if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
- // {
- // continue;
- // }
- // }
- // if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
- // {
- // if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
- // {
- // break;
- // }
- // }
- // xvectorPP = GetPoint( xpathsection[i].mpRoad);
- xvectorPP = GetPoint( xpathsection[i],fspace);
- xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
- // std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
- // <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
- // std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
- int j;
- for(j=0;j<xvPP.size();j++)
- {
- PlanPoint pp;
- pp = xvPP.at(j);
- // pp.hdg = pp.hdg +M_PI;
- xvectorPPs.push_back(pp);
- }
- }
- xvectorPP = GetPoint(xpathsection[npathlast],fspace);
- // xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
- int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
- xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
- int nlastsize;
- if(xpathsection[npathlast].mainsel<0)
- {
- nlastsize = indexend;
- }
- else
- {
- if(indexend>0) nlastsize = xvPP.size() - indexend;
- else nlastsize = xvPP.size();
- }
- for(i=0;i<nlastsize;i++)
- {
- xvectorPPs.push_back(xvPP.at(i));
- }
- //Find StartPoint
- // if
- // int a = 1;
- return xvectorPPs;
- }
- std::vector<PlanPoint> gTempPP;
- int getPoint(double q[3], double x, void* user_data) {
- // printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
- PlanPoint pp;
- pp.x = q[0];
- pp.y = q[1];
- pp.hdg = q[2];
- pp.dis = x;
- pp.speed = *((double *)user_data);
- gTempPP.push_back(pp);
- // std::cout<<pp.x<<" "<<" "<<pp.y<<" "<<pp.hdg<<std::endl;
- return 0;
- }
- /**
- * @brief getlanesel
- * @param nSel
- * @param pLaneSection
- * @param nlr
- * @return
- */
- int getlanesel(int nSel,LaneSection * pLaneSection,int nlr)
- {
- int nlane = nSel;
- int mainselindex = 0;
- if(nlr == 2)nlane = nlane*(-1);
- int nlanecount = pLaneSection->GetLaneCount();
- int i;
- int nTrueSel = nSel;
- if(nTrueSel <= 1)nTrueSel= 1;
- int nCurSel = 1;
- if(nlr == 2)nCurSel = nCurSel *(-1);
- bool bOK = false;
- int nxsel = 1;
- int nlasttid = 0;
- bool bfindlast = false;
- while(bOK == false)
- {
- bool bHaveDriving = false;
- int ncc = 0;
- for(i=0;i<nlanecount;i++)
- {
- Lane * pLane = pLaneSection->GetLane(i);
- if(strncmp(pLane->GetType().data(),"driving",255) == 0 )
- {
- if((nlr == 1)&&(pLane->GetId()>0))
- {
- ncc++;
- }
- if((nlr == 2)&&(pLane->GetId()<0))
- {
- ncc++;
- }
- if(ncc == nxsel)
- {
- bHaveDriving = true;
- bfindlast = true;
- nlasttid = pLane->GetId();
- break;
- }
- }
- }
- if(bHaveDriving == true)
- {
- if(nxsel == nTrueSel)
- {
- return nlasttid;
- }
- else
- {
- nxsel++;
- }
- }
- else
- {
- if(bfindlast)
- {
- return nlasttid;
- }
- else
- {
- std::cout<<"can't find lane."<<std::endl;
- return 0;
- }
- //Use Last
- }
- }
- return 0;
- int k;
- bool bFind = false;
- while(bFind == false)
- {
- for(k=0;k<nlanecount;k++)
- {
- Lane * pLane = pLaneSection->GetLane(k);
- if((nlane == pLane->GetId())&&(strncmp(pLane->GetType().data(),"driving",255) == 0))
- {
- bFind = true;
- mainselindex = k;
- break;
- }
- }
- if(bFind)continue;
- if(nlr == 1)nlane--;
- else nlane++;
- if(nlane == 0)
- {
- std::cout<<" Fail. can't find lane"<<std::endl;
- break;
- }
- }
- return nlane;
- }
- void checktrace(std::vector<PlanPoint> & xPlan)
- {
- int i;
- int nsize = xPlan.size();
- for(i=1;i<nsize;i++)
- {
- double dis = sqrt(pow(xPlan.at(i).x - xPlan.at(i-1).x,2) + pow(xPlan.at(i).y - xPlan.at(i-1).y,2));
- if(dis > 0.3)
- {
- double hdg = CalcHdg(QPointF(xPlan.at(i-1).x,xPlan.at(i-1).y),QPointF(xPlan.at(i).x,xPlan.at(i).y));
- int ncount = dis/0.1;
- int j;
- for(j=1;j<ncount;j++)
- {
- double x, y;
- PlanPoint pp;
- pp.x = xPlan.at(i-1).x + j*0.1 *cos(hdg);
- pp.y = xPlan.at(i-1).y + j*0.1 *sin(hdg);
- pp.hdg = hdg;
- xPlan.insert(xPlan.begin()+i+j-1,pp);
- }
- qDebug("dis is %f",dis);
- }
- }
- }
- void ChangeLane(std::vector<PlanPoint> & xvectorPP)
- {
- int i = 0;
- int nsize = xvectorPP.size();
- int nstart = -1;
- int nend = -1;
- for(i=0;i<nsize;i++)
- {
- if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
- {
- }
- else
- {
- int k;
- for(k=i;k<=(nsize-1);k++)
- {
- if((fabs(xvectorPP[k].mfSecx - xvectorPP[k].x)<0.001)&&(fabs(xvectorPP[k].mfSecy - xvectorPP[k].y)<0.001))
- {
- break;
- }
- }
- if(k>(nsize-1))k=nsize-1;
- int nnum = k-i;
- int nchangepoint = 300;
- double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
- +pow(xvectorPP[k].y - xvectorPP[i].y,2));
- const double fMAXCHANGE = 50;
- if(froadlen<fMAXCHANGE)
- {
- nchangepoint = nnum;
- }
- else
- {
- nchangepoint = (fMAXCHANGE/froadlen) * nnum;
- }
- qDebug(" road %d len is %f changepoint is %d nnum is %d",
- xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);
- nstart = i + nnum/2 -nchangepoint/2;
- nend = i+nnum/2 + nchangepoint/2;
- if(nnum<300)
- {
- nstart = i;
- nend = k;
- }
- int j;
- for(j=i;j<nstart;j++)
- {
- xvectorPP[j].x = xvectorPP[j].mfSecx;
- xvectorPP[j].y = xvectorPP[j].mfSecy;
- }
- nnum = nend - nstart;
- int nlast = k-1;
- if(k==(nsize-1))nlast = k;
- for(j=nstart;j<=nlast;j++)
- {
- double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
- +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
- double foff = 0;
- if(j<nend)
- foff = fdis *(j-nstart)/nnum;
- else
- foff = fdis;
- if(xvectorPP[j].nlrchange == 1)
- {
- // std::cout<<"foff is "<<foff<<std::endl;
- xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
- xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
- xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft - foff; //+ fdis
- }
- else
- {
- xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
- xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
- xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft +foff;//- fdis
- }
- }
- i =k;
- }
- }
- if(nstart != -1)
- {
- for(i=0;i<nstart;i++)
- {
- xvectorPP[i].nlrchange = 0;
- if(i>nsize)break;
- }
- }
- if(nend >0)
- {
- for(i=nend;i<nsize;i++)
- {
- xvectorPP[i].nlrchange = 0;
- if(i>nsize)break;
- }
- }
- }
- #include <QFile>
- /**
- * @brief MakePlan 全局路径规划
- * @param pxd 有向图
- * @param pxodr OpenDrive地图
- * @param x_now 当前x坐标
- * @param y_now 当前y坐标
- * @param head 当前航向
- * @param x_obj 目标点x坐标
- * @param y_obj 目标点y坐标
- * @param obj_dis 目标点到路径的距离
- * @param srcnearthresh 当前点离最近路径的阈值,如果不在这个范围,寻找失败
- * @param dstnearthresh 目标点离最近路径的阈值,如果不在这个范围,寻找失败
- * @param nlanesel 车道偏好,1为内车道,数值越大越偏外,如果数值不满足,会选择最靠外的。
- * @param xPlan 返回的轨迹点
- * @return 0 成功 <0 失败
- */
- int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
- const double x_obj,const double y_obj,const double & obj_dis,
- const double srcnearthresh,const double dstnearthresh,
- const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth )
- {
- double s;double nearx;
- double neary;double nearhead;
- Road * pRoad;GeometryBlock * pgeob;
- Road * pRoad_obj;GeometryBlock * pgeob_obj;
- double s_obj;double nearx_obj;
- double neary_obj;double nearhead_obj;
- int lr_end = 2;
- int lr_start = 2;
- double fs1,fs2;
- // int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
- std::vector<iv::nearroad> xvectornearstart;
- std::vector<iv::nearroad> xvectornearend;
- GetNearPointWithHead(x_now,y_now,head,pxodr,xvectornearstart,srcnearthresh,true);
- GetNearPointWithHead(x_obj,y_obj,0,pxodr,xvectornearend,dstnearthresh,false);
- if(xvectornearstart.size()<1)
- {
- std::cout<<" plan fail. can't find start point."<<std::endl;
- return -1;
- }
- if(xvectornearend.size() < 1)
- {
- std::cout<<" plan fail. can't find end point."<<std::endl;
- return -2;
- }
- std::cout<<" start size: "<<xvectornearstart.size()<<std::endl;
- std::cout<<" end size: "<<xvectornearend.size()<<std::endl;
- std::vector<std::vector<PlanPoint>> xvectorplans;
- std::vector<double> xvectorroutedis;
- int i;
- int j;
- for(i=0;i<(int)xvectornearstart.size();i++)
- {
- for(j=0;j<(int)xvectornearend.size();j++)
- {
- iv::nearroad * pnearstart = &xvectornearstart[i];
- iv::nearroad * pnearend = &xvectornearend[j];
- pRoad = pnearstart->pObjRoad;
- pgeob = pnearstart->pgeob;
- pRoad_obj = pnearend->pObjRoad;
- pgeob_obj = pnearend->pgeob;
- lr_start = pnearstart->lr;
- lr_end = pnearend->lr;
- std::cout<<" lr start: "<<lr_start<<" lr end "<<lr_end<<std::endl;
- nearx = pnearstart->nearx;
- neary = pnearstart->neary;
- nearx_obj = pnearend->nearx;
- neary_obj = pnearend->neary;
- bool bNeedDikstra = true;
- if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
- {
- std::vector<PlanPoint> xvPP = GetPoint(pRoad);
- int nindexstart = indexinroadpoint(xvPP,nearx,neary);
- int nindexend = indexinroadpoint(xvPP,nearx_obj,neary_obj);
- int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
- AddSignalMark(pRoad,nlane,xvPP);
- if((nindexend >nindexstart)&&(lr_start == 2))
- {
- bNeedDikstra = false;
- }
- if((nindexend <nindexstart)&&(lr_start == 1))
- {
- bNeedDikstra = false;
- }
- if(bNeedDikstra == false)
- {
- std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
- std::vector<int> xvectorindex1;
- xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
- double foff = getoff(pRoad,nlane);
- foff = foff + 0.0;
- std::vector<PlanPoint> xvectorPP;
- int i = nindexstart;
- while(i!=nindexend)
- {
- if(lr_start == 2)
- {
- PlanPoint pp = xvPP.at(i);
- foff = getoff(pRoad,nlane,pp.mS);// getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
- // std::cout<<"foff: "<<foff<<std::endl;
- pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
- pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
- pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
- pp.mfDisToLaneLeft = pp.mWidth/2.0;
- pp.lanmp = 0;
- pp.mfDisToRoadLeft = foff *(-1.0);
- pp.mfRoadWidth = GetRoadWidth(pRoad,nlane,pp.mS);
- pp.mLaneCur = abs(nlane);
- pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
- pp.mLaneCount = pRoad->GetRightDrivingLaneCount(pp.mS);
- GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
- xvectorPP.push_back(pp);
- i++;
- }
- else
- {
- PlanPoint pp = xvPP.at(i);
- foff = getoff(pRoad,nlane,pp.mS);// getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
- pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
- pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
- pp.hdg = pp.hdg + M_PI;
- pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
- pp.mfDisToLaneLeft = pp.mWidth/2.0;
- pp.lanmp = 0;
- pp.mfDisToRoadLeft = foff;
- pp.mfRoadWidth = GetRoadWidth(pRoad,nlane,pp.mS);
- pp.mLaneCur = abs(nlane);
- pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
- pp.mLaneCount = pRoad->GetLeftDrivingLaneCount(pp.mS);
- GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
- xvectorPP.push_back(pp);
- i--;
- }
- }
- for(i=0;i<(int)xvectorPP.size();i++)
- {
- xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
- }
- pathsection xps;
- xps.mbStartToMainChange = false;
- xps.mbMainToEndChange = false;
- // CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
- xPlan = xvectorPP;
- xvectorplans.push_back(xvectorPP);
- xvectorroutedis.push_back(xvectorPP.size() * 0.1);
- }
- }
- if(bNeedDikstra)
- {
- bool bSuc = false;
- std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
- if(xpath.size()<1)
- {
- continue;
- }
- if(bSuc == false)
- {
- continue;
- }
- double flen = pxd->getpathlength(xpath);
- std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
- std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
- head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
- ChangeLane(xvectorPP);
- xvectorplans.push_back(xvectorPP);
- xvectorroutedis.push_back(flen);
- }
- }
- }
- if(xvectorplans.size() > 0)
- {
- if(xvectorplans.size() == 1)
- {
- xPlan = xvectorplans[0];
- }
- else
- {
- int nsel = 0;
- double fdismin = xvectorroutedis[0];
- for(i=1;i<(int)xvectorroutedis.size();i++)
- {
- if(xvectorroutedis[i]<fdismin)
- {
- nsel = i;
- }
- }
- xPlan = xvectorplans[nsel];
- }
- return 0;
- }
- std::cout<<" plan fail. can't find path."<<std::endl;
- return -1000;
- }
|