#include "globalplan.h" #include "limits" #include #include #include #include #include #include #include #include using namespace Eigen; extern "C" { #include "dubins.h" } static void AddSignalMark(Road * pRoad, int nlane,std::vector & 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<pline->GetLength())||(dis2>pline->GetLength())) //Outoff line { // std::cout<<" out line"<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 "< 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]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 "<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(sGetLength()) { pspiral->GetCoords(s0+s,x,y,hdg); s = s+0.1; double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow)); if(fdisGetX(); yold = parc->GetY(); double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow)); if(fdisGetHdg(); nearx = parc->GetX(); neary = 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(); double hdg = CalcHdg(QPointF(xold,yold),QPointF(x,y)); double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow)); if(fdisGetS(); 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::infinity(); s = dismin; int i; double frels; for(i=0;iGetRoadCount();i++) { int j; Road * proad = pxodr->GetRoad(i); double nx,ny,nh; for(j=0;jGetGeometryBlockCount();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,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx, double & neary,double & nearhead,const double nearthresh,double &froads) { double dismin = std::numeric_limits::infinity(); s = dismin; int i; double frels; std::vector xvectorroad; std::vector xvectordismin; std::vector xvecotrnearx; std::vector xvectorneary; std::vector xvectornearhead; std::vector xvectors; std::vector xvectorgeob; std::vector xvectorfrels; double VECTORTHRESH = 5; for(i=0;iGetRoadCount();i++) { int j; Road * proad = pxodr->GetRoad(i); double nx,ny,nh; bool bchange = false; double localdismin = std::numeric_limits::infinity(); double localnearx = 0; double localneary = 0; double localnearhead = 0; double locals = 0; double localfrels = 0; GeometryBlock * pgeolocal; for(j=0;jGetGeometryBlockCount();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 < dismin) { dismin = dis; nearx = nx; neary = ny; nearhead = nh; s = dis; froads = frels; *pObjRoad = proad; *pgeo = pgb; } if((dis nearthresh)return -1; if((*pObjRoad)->GetLaneSectionCount()>0) { LaneSection * pLS = (*pObjRoad)->GetLaneSection(0); if((*pObjRoad)->GetLaneSection(0)->GetLeftLaneCount()&&(*pObjRoad)->GetLaneSection(0)->GetRightLaneCount()>0) { return 0; } else { double headdiff = hdg - 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*3.0/2.0))&&(pLS->GetRightLaneCount()>0)) { return 0; } else { if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0)&&(pLS->GetLeftLaneCount()>0)) { return 0; } else { bool bHaveSame = false; for(i=0;iGetLaneSectionCount()<1)continue; bool bNeedFind = false; if(bHaveSame == false)bNeedFind = true; else { if(xvectordismin[i]= 2.0*M_PI)fdiff = fdiff - 2.0*M_PI; bool bUse = false; LaneSection * pLS = xvectorroad[i]->GetLaneSection(0); if(((fdiff M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0)) { bUse = true; } if(((fdiff >=M_PI/2.0)&&(fdiff <= M_PI*3.0/2.0))&&(pLS->GetLeftLaneCount()>0)) { bUse = true; } if(bUse) { dismin = xvectordismin[i]; nearx = xvecotrnearx[i]; neary = xvectorneary[i]; nearhead = xvectornearhead[i]; s = xvectors[i]; froads = xvectorfrels[i]; *pObjRoad = xvectorroad[i]; *pgeo = xvectorgeob[i]; bHaveSame = true; std::cout<<"change road. "<= 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 "<GetS()<<" dis is "<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 getlinepoint(GeometryLine * pline,const double fspace = 0.1) { std::vector xvectorPP; int i; double s = pline->GetLength(); int nsize = s/fspace; if(nsize ==0)nsize = 1; for(i=0;iGetX() + i *fspace * cos(pline->GetHdg()); y = pline->GetY() + i *fspace * sin(pline->GetHdg()); PlanPoint pp; pp.x = x; pp.y = y; pp.dis = i*0.1; pp.hdg = pline->GetHdg(); pp.mS = pline->GetS() + i*fspace; xvectorPP.push_back(pp); } return xvectorPP; } static std::vector getarcpoint(GeometryArc * parc,const double fspace = 0.1) { std::vector 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;iGetCurvature() > 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*0.1; pp.mS = parc->GetS() + i*0.1; xvectorPP.push_back(pp); } return xvectorPP; } static std::vector getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1) { double x,y,hdg; double s = 0.0; double s0 = pspiral->GetS(); std::vector xvectorPP; PlanPoint pp; while(sGetLength()) { 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 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 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 getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1) { double s = 0.1; std::vector 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.1; 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 a = parc->GetS(); while(s < parc->GetLength()) // while(s<1.0) { double len = parc->GetLength(); double x, y,xtem,ytem; // xtem = parc->GetuA() + parc->GetuB() * s * len + parc->GetuC() * s*s *pow(len,2) + parc->GetuD() * s*s*s *pow(len,3); // ytem = parc->GetvA() + parc->GetvB() * s* len + parc->GetvC() * s*s *pow(len,2) + parc->GetvD() * s*s*s *pow(len,3); xtem = ua + ub * s + uc * s*s + ud * s*s*s ; ytem = va + vb * s + vc * s*s + vd * s*s*s ; x = xtem*cos(hdg0) - ytem * sin(hdg0) + parc->GetX(); y = xtem*sin(hdg0) + ytem * cos(hdg0) + parc->GetY(); // x= xtem + parc->GetX(); // y = ytem + parc->GetY(); // x = xtem*cos(parc->GetHdg()) + ytem * sin(parc->GetHdg()) + parc->GetX(); // y = -xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY(); double hdg = CalcHdg(QPointF(xold,yold),QPointF(x,y)); pp.x = x; pp.y = y; pp.hdg = hdg; double disx = sqrt(pow(x-xold,2)+ pow(y-yold,2)); /* if(disx > 0.1)s = s+ disx; else s = s+ 0.5*/; s = s+ fspace; // s = s+ sqrt(pow(x-xold,2)+ pow(y-yold,2)); xold = x; yold = y; // s = s+ dt; pp.dis = pp.dis + disx;; pp.mS = parc->GetS() + s; xvectorPP.push_back(pp); // std::cout<<" s is "< GetPoint(pathsection xpath,const double fspace = 0.1) { Road * pRoad = xpath.mpRoad; 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(); } std::vector xvectorPPS; double s = 0; int i; for(i=0;iGetGeometryBlockCount();i++) { GeometryBlock * pgb = pRoad->GetGeometryBlock(i); RoadGeometry * prg = pgb->GetGeometryAt(0); std::vector xvectorPP; if(i<(pRoad->GetGeometryBlockCount() -0)) { if(prg->GetS()>s_end) { continue; } if((prg->GetS() + prg->GetLength())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()GetLength() -(s_start - prg->GetS()); } if((prg->GetS() + prg->GetLength())>s_end) { s1 = s_end - prg->GetS(); } for(j=0;jGetS()) >= 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())GetRoadId().data()); // xvectorPPS.push_back(pp); // } // else // { // if(prg->GetS()GetS())) // { // pp.dis = s + pp.dis; // pp.nRoadID = atoi(pRoad->GetRoadId().data()); // xvectorPPS.push_back(pp); // } // } // } } s = s+ s1; } return xvectorPPS; } std::vector GetPoint(Road * pRoad) { std::vector xvectorPPS; double s = 0; int i; for(i=0;iGetGeometryBlockCount();i++) { GeometryBlock * pgb = pRoad->GetGeometryBlock(i); RoadGeometry * prg = pgb->GetGeometryAt(0); std::vector 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;jGetRoadId().data()); xvectorPPS.push_back(pp); } s = s+ s1; } return xvectorPPS; } int indexinroadpoint(std::vector xvectorPP,double x,double y) { int nrtn = 0; int i; int dismin = 1000; for(i=0;i 10.0) { std::cout<<"indexinroadpoint near point is error. dis is "<GetLaneSection(0)->GetLaneCount(); for(i=0;iGetLaneSection(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;inlane;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 xvectorlwa,double s) { double frtn = 0; int i; int nlanecount = xvectorlwa.size(); for(i=0;iA + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s; break; } } return frtn; } std::vector GetLaneWidthABCD(Road * pRoad) { std::vector 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;iGetLane(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 xvectorLWA,std::vector 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 "< xvectorLWA,int nlane,double s) { double fwidth = 0; int i; double a,b,c,d; int nsize = xvectorLWA.size(); for(i=0;i0) { 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) { 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 int nlanecount = pLS->GetLaneCount(); for(i=0;iGetLane(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 0) { if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0) { ntotal++; if(nlaneid > nlane)nori++; } } } } return 0; } std::vector GetLWIndex(std::vector xvectorLWA,int nlane) { std::vector xvectorindex; int i; if(nlane >= 0) { for(i=0;i<=nlane;i++) { int j; int nsize = xvectorLWA.size(); for(j=0;j=nlane;i--) { int j; int nsize = xvectorLWA.size(); for(j=0;j & 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=0)&&(i=0)&&(iGetLaneSectionCount(); double s_section = 0; double foff = 0; for(i=0;iGetLaneSection(i); if(i<(nLSCount -1)) { if(pRoad->GetLaneSection(i+1)->GetS()GetS(); int nlanecount = pLS->GetLaneCount(); int j; for(j=0;jGetLane(j); int k; double s_lane = s-s_section; if(pLane->GetId() != 0) { for(k=0;kGetLaneWidthCount();k++) { if(k<(pLane->GetLaneWidthCount()-1)) { if((pLane->GetLaneWidth(k+1)->GetS()+s_section)GetLaneWidth(k)->GetS(); break; } LaneWidth * pLW = pLane->GetLaneWidth(k); if(pLW == 0) { std::cout<<"not find LaneWidth"<GetA() + pLW->GetB() * fds +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3); if(nlane == pLane->GetId()) { foff = foff + fwidth/2.0; } if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId()))) { foff = foff + fwidth; } } } break; } if(nlane<0)foff = foff*(-1); return foff; } std::vector GetLanePoint(pathsection xps,std::vector xvPP,const double fvehiclewidth) { std::vector xvectorPP; std::vector xvectorLWA = GetLaneWidthABCD(xps.mpRoad); std::vector 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 "<GetRoadId()<xps.secondsel) { pp.nlrchange = 1; } else { pp.nlrchange = 2; } } else { pp.mfSecx = pp.x ; pp.mfSecy = pp.y ; } pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS); pp.mfDisToLaneLeft = pp.mWidth/2.0; pp.lanmp = 0; pp.mfDisToRoadLeft = off1*(-1); pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS); GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed); xvectorPP.push_back(pp); } } else { for(i=0;ixps.secondsel) { pp.nlrchange = 2; } else { pp.nlrchange = 1; } } else { pp.mfSecx = pp.x ; pp.mfSecy = pp.y ; } 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=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 0) { std::vector xvvPP; int nvsize = xvectorPP.size(); for(i=0;iGetLaneSection(xps.msectionid) AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP); return xvectorPP; } static void AddSignalMark(Road * pRoad, int nlane,std::vector & xvectorPP) { if(pRoad->GetSignalCount() == 0)return; int nsigcount = pRoad->GetSignalCount(); int i; for(i=0;iGetSignal(i); int nfromlane = -100; int ntolane = 100; 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 GetPlanPoint(std::vector 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 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 xvectorPP = GetPoint( xpathsection[0].mpRoad); std::vector xvectorPP = GetPoint( xpathsection[0],fspace); int indexstart = indexinroadpoint(xvectorPP,nearx,neary); std::vector xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth); if(xpathsection[0].mainsel < 0) { for(i=indexstart;i= 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: "<GetRoadId().data()<<" size is "<0) nlastsize = xvPP.size() - indexend; else nlastsize = xvPP.size(); } for(i=0;i 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<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;iGetLane(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."<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"< & xPlan) { int i; int nsize = xPlan.size(); for(i=1;i 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 & xvectorPP) { int i = 0; int nsize = xvectorPP.size(); for(i=0;i /** * @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 & xPlan,const double fvehiclewidth ) { double s;double nearx; double neary;double nearhead; Road * pRoad;GeometryBlock * pgeob; double fs1,fs2; // int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh); int nfind = GetNearPointWithHead(x_now,y_now,head,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh,fs1); if(nfind < 0)return -1; fs1 =fs1 + pgeob->GetGeometryAt(0)->GetS(); double s_obj;double nearx_obj; double neary_obj;double nearhead_obj; Road * pRoad_obj;GeometryBlock * pgeob_obj; int nfind_obj = GetNearPoint(x_obj,y_obj,pxodr,&pRoad_obj,&pgeob_obj,s_obj,nearx_obj,neary_obj, nearhead_obj,dstnearthresh,fs2); if(nfind_obj < 0)return -2; fs2 = fs2 + pgeob_obj->GetGeometryAt(0)->GetS(); //计算终点在道路的左侧还是右侧 int lr_end = 2; double hdg_end; hdg_end = CalcHdg(QPointF(nearx_obj,neary_obj),QPointF(x_obj,y_obj)); double hdgdiff = nearhead_obj - hdg_end; while(hdgdiff<0)hdgdiff= hdgdiff + 2.0*M_PI; while(hdgdiff >= 2.0*M_PI)hdgdiff = hdgdiff- 2.0*M_PI; if(hdgdiff<= M_PI) { lr_end = 2; } else { lr_end = 1; } if((pRoad_obj->GetLaneSection(0)->GetLeftLaneCount()<1)&&(lr_end == 1)) { lr_end = 2; } if((pRoad_obj->GetLaneSection(0)->GetRightLaneCount()<1)&&(lr_end == 2)) { lr_end = 2; } int lr_start = SelectRoadLeftRight(pRoad,head,nearhead); bool bNeedDikstra = true; if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1)) { std::vector 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 xvectorLWA = GetLaneWidthABCD(pRoad); std::vector xvectorindex1; xvectorindex1 = GetLWIndex(xvectorLWA,nlane); double foff = getoff(pRoad,nlane); foff = foff + 0.0; std::vector xvectorPP; int i = nindexstart; while(i!=nindexend) { if(lr_start == 2) { PlanPoint pp = xvPP.at(i); foff = 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.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS); pp.mfDisToLaneLeft = pp.mWidth/2.0; pp.lanmp = 0; pp.mfDisToRoadLeft = foff *(-1.0); pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS); GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed); xvectorPP.push_back(pp); i++; } else { PlanPoint pp = xvPP.at(i); foff = 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(xvectorLWA,nlane,pp.mS); GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed); xvectorPP.push_back(pp); i--; } } pathsection xps; xps.mbStartToMainChange = false; xps.mbMainToEndChange = false; // CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0); xPlan = xvectorPP; } } if(bNeedDikstra) { std::vector xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,fs1,fs2); double flen = pxd->getpathlength(xpath); std::vector xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel); std::vector 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); xPlan = xvectorPP; } int i; // QFile xFile; // xFile.setFileName("/home/yuchuli/tempgps.txt"); // xFile.open(QIODevice::ReadWrite); // for(i<0;iCheckIfLine())return -5; //temp,only line plan; int nlane = nlanesel; if(nlane <= 0 )nlane = 1; if(nlane >= pRoad->GetLaneSection(0)->GetLaneCount()) { nlane = pRoad->GetLaneSection(0)->GetLaneCount()-1; } double bianxiandis = 0; double bianxiandis_obj = 0; double park_x,park_y;//Park point bianxiandis = pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(1)/2.0; // int i = 1; for(i=1;iGetLaneSection(0)->GetLane(i)->GetWidthValue(1)/2.0 +pRoad->GetLaneSection(0)->GetLane(i+1)->GetWidthValue(1)/2.0; } double bianxianx1 = bianxiandis; bianxiandis = fabs(s - bianxiandis); bianxiandis_obj = 0; int ag = pRoad->GetLaneSection(0)->GetLaneCount(); for(i=(nlane +1);iGetLaneSection(0)->GetLaneCount();i++) { bianxiandis_obj = bianxiandis_obj +pRoad->GetLaneSection(0)->GetLane(i-1)->GetWidthValue(1)/2.0 +pRoad->GetLaneSection(0)->GetLane(i)->GetWidthValue(1)/2.0; } // double x_objreal = nearx_obj + bianxiandis_obj * cos(nearhead_obj - M_PI/2.0); // park_x = nearx_obj + (bianxiandis_obj +bianxianx1 + pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(0)/2.0)*cos(nearhead_obj -M_PI/2.0); // park_y = neary_obj + (bianxiandis_obj +bianxianx1+ pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(0)/2.0)*sin(nearhead_obj -M_PI/2.0); park_x = nearx_obj + (bianxiandis_obj +bianxianx1 )*cos(nearhead_obj -M_PI/2.0); park_y = neary_obj + (bianxiandis_obj +bianxianx1 )*sin(nearhead_obj -M_PI/2.0); const double bianxian_ang = 0.2; double src_len,dst_len; src_len = bianxiandis/atan(bianxian_ang); dst_len = bianxiandis_obj/atan(bianxian_ang); double xl1,yl1,xl2,yl2; xl1 = nearx + src_len * cos(nearhead); yl1 = neary + src_len * sin(nearhead); xl2 = nearx_obj - dst_len * cos(nearhead_obj); yl2 = neary_obj - dst_len * sin(nearhead_obj); xl1 = xl1 + bianxianx1 * cos(nearhead - M_PI/2.0); yl1 = yl1 + bianxianx1 * sin(nearhead - M_PI/2.0); xl2 = xl2 + bianxianx1 * cos(nearhead_obj - M_PI/2.0); yl2 = yl2 + bianxianx1 * sin(nearhead_obj - M_PI/2.0); double q0[3],q1[3]; gTempPP.clear(); q0[0] = x_now;q0[1] = y_now; q0[2] = head; q1[0] = xl1;q1[1] =yl1;q1[2] = nearhead; int indexp = 0; double turning_radius = 15.0; double speedvalue = 3; DubinsPath path; dubins_shortest_path( &path, q0, q1, turning_radius); dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue); for(i=indexp;iGetLaneSection(0)->GetLane(nlane)->GetLaneSpeedCount()>1) speedvalue = pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeed(1)->GetMax(); else speedvalue = 10.0; memcpy(q0,q1,3*sizeof(double)); q1[0]=xl2; q1[1]=yl2; q1[2]=nearhead; dubins_shortest_path( &path, q0, q1, turning_radius); dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue); for(i=indexp;i0;i--) { gTempPP.at(i).speed = vx; vx = vx + 0.03; gTempPP.at(i).lanmp = -1; if(vx > speedvalue)break; } speedvalue = 3.0; memcpy(q0,q1,3*sizeof(double)); q1[0]=park_x; q1[1]=park_y; q1[2]=nearhead_obj; dubins_shortest_path( &path, q0, q1, turning_radius); dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue); for(i=indexp;i 0) { hdgsrc = asin((yl1-y_now)/ldis1); if(xl1 < x_now)hdgsrc = M_PI - hdgsrc; if(hdgsrc < 0)hdgsrc = hdgsrc + 2.0 * M_PI; } double ldis2 = sqrt(pow(park_x-xl2,2)+pow(park_y-yl2,2)); if(ldis2 > 0) { hdgdst = asin((park_y - yl2)/ldis2); if(park_x < xl2)hdgdst = M_PI - hdgdst; if(hdgdst < 0)hdgdst = hdgdst + 2.0*M_PI; } double xv,yv,speed; i = 0; double disx = 0.0; const double step = 0.1; xv = x_now; yv = y_now; pp.x = xv;pp.y = yv;pp.hdg = hdgsrc;pp.speed= 3.0;pp.lanmp = 1;xPlan.push_back(pp); // qDebug("%d %f %f",i,xv,yv); if(ldis1 > 0) { while((disx+step) < ldis1) { disx = disx + step; i++; xv = x_now + disx * cos(hdgsrc); yv = y_now + disx * sin(hdgsrc); pp.x = xv;pp.y = yv;pp.hdg = hdgsrc;pp.speed= 3.0;xPlan.push_back(pp); // qDebug("%d %f %f",i,xv,yv); } } i++; xv = xl1;yv = yl1; // qDebug("1:%d %f %f",i,xv,yv); pp.x = xv;pp.y = yv;pp.hdg = hdgsrc;pp.speed= 3.0;pp.lanmp = 0;xPlan.push_back(pp); double dis2p = sqrt(pow(xl2-xl1,2) + pow(yl2 -yl1,2)); disx = 0; if(dis2p > 0) { while((disx+step) < dis2p) { disx = disx + step; i++; xv = xl1 + disx * cos(nearhead); yv = yl1 + disx * sin(nearhead); if(pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeedCount()>1) speed = pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeed(1)->GetMax(); else speed = 10.0; if((dis2p -disx)<((speed - 3.0)*5)){ speed = 3.0; pp.lanmp = -1; } else { pp.lanmp = 0; } pp.x = xv;pp.y = yv;pp.hdg = nearhead;pp.speed= speed;xPlan.push_back(pp); // qDebug("%d %f %f speed is %f",i,xv,yv,speed); } } i++; xv = xl2;yv = yl2; pp.x = xv;pp.y = yv;pp.hdg = hdgdst;pp.speed= 3.0;pp.lanmp = -1;xPlan.push_back(pp); // qDebug("2: %d %f %f",i,xv,yv); disx = 0; if(ldis2 > 0) { while((disx+step) < ldis2) { disx = disx + step; i++; xv = xl2 + disx * cos(hdgdst); yv = yl2 + disx * sin(hdgdst); speed = 3.0; pp.x = xv;pp.y = yv;pp.hdg = hdgdst;pp.speed= 3.0;pp.lanmp = -1;xPlan.push_back(pp); // qDebug("%d %f %f",i,xv,yv); } } i++; xv = park_x;yv = park_y; pp.x = xv;pp.y = yv;pp.hdg = nearhead_obj;pp.speed= 0.0;pp.lanmp = 0;xPlan.push_back(pp); // qDebug("obj: %d %f %f",i,xv,yv); disx = 0; while((disx+step) < 30) { disx = disx + step; i++; xv = park_x + disx * cos(nearhead); yv = park_y + disx * sin(nearhead); speed = 0.0; pp.x = xv;pp.y = yv;pp.hdg = nearhead_obj;pp.speed= speed;pp.lanmp = 0;xPlan.push_back(pp); // qDebug("%d %f %f",i,xv,yv); } }