modularization/src/driver/driver_map_xodrload/globalplan.cpp

#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();
    }

    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(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 dismin = std::numeric_limits<double>::infinity();
    s = dismin;
    int i;
    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 = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
                break;
            case 1:
                dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh);
                break;
            case 2:  //arc
                dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh);
                break;

            case 3:
                dis = GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh);
                break;
            case 4:
                dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh);
                break;
            default:
                dis = 100000.0;
                break;
            }

            if(dis < dismin)
            {
                dismin = dis;
                nearx = nx;
                neary = ny;
                nearhead = nh;
                s = dis;
                *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 dismin = std::numeric_limits<double>::infinity();
    s = dismin;
    int i;
    std::vector<Road * > xvectorroad;
    std::vector<double > xvectordismin;
    std::vector<double > xvecotrnearx;
    std::vector<double > xvectorneary;
    std::vector<double > xvectornearhead;
    std::vector<double > xvectors;
    std::vector<GeometryBlock *> xvectorgeob;
    double VECTORTHRESH = 5;
    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;
        GeometryBlock * pgeolocal;

        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 = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
                break;
            case 1:
                dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh);
                break;
            case 2:  //arc
                dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh);
                break;

            case 3:
                dis = 100000.0;
                break;
            case 4:
                dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh);
                break;
            default:
                dis = 100000.0;
                break;
            }

            if(dis < dismin)
            {
                dismin = dis;
                nearx = nx;
                neary = ny;
                nearhead = nh;
                s = dis;
                *pObjRoad = proad;
                *pgeo = pgb;

            }

            if((dis<VECTORTHRESH) &&(dis<localdismin))
            {
                localdismin = dis;
                localnearx = nx;
                localneary = ny;
                localnearhead = nh;
                locals = dis;
                pgeolocal = pgb;
                bchange = true;
            }




        }

        if(bchange)
        {
            xvectorroad.push_back(proad);
            xvecotrnearx.push_back(localnearx);
            xvectorneary.push_back(localneary);
            xvectordismin.push_back(localdismin);
            xvectors.push_back(locals);
            xvectorgeob.push_back(pgeolocal);
            xvectornearhead.push_back(localnearhead);
        }
    }


    if(s > 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/2.0)||(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;i<xvectorroad.size();i++)
                    {
                        if(xvectorroad[i]->GetLaneSectionCount()<1)continue;
                        bool bNeedFind = false;
                        if(bHaveSame == false)bNeedFind = true;
                        else
                        {
                            if(xvectordismin[i]<dismin)bNeedFind = true;
                        }
                        if(bNeedFind)
                        {
                            double fdiff = hdg - xvectornearhead[i];
                            while(fdiff < 2.0*M_PI) fdiff = fdiff + 2.0*M_PI;
                            while(fdiff >= 2.0*M_PI)fdiff = fdiff - 2.0*M_PI;

                            bool bUse = false;
                            LaneSection * pLS = xvectorroad[i]->GetLaneSection(0);
                            if(((fdiff <M_PI/2.0)||(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];
                                *pObjRoad = xvectorroad[i];
                                *pgeo = xvectorgeob[i];
                                bHaveSame = true;
                                std::cout<<"change road. "<<std::endl;
                            }
                        }
                    }
                }
            }
        }
    }

    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)
{
    std::vector<PlanPoint> xvectorPP;
    int i;
    double s = pline->GetLength();
    int nsize = s/0.1;

    for(i=0;i<nsize;i++)
    {
        double x,y;
        x = pline->GetX() + i *0.1 * cos(pline->GetHdg());
        y = pline->GetY() + i *0.1 * 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*0.1;
        xvectorPP.push_back(pp);
    }
    return xvectorPP;
}

static std::vector<PlanPoint> getarcpoint(GeometryArc * parc)
{
    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 = 0.1/R;
    int nsize = parc->GetLength()/0.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*0.1;
        pp.mS = parc->GetS() + i*0.1;
        xvectorPP.push_back(pp);
    }
    return xvectorPP;
}


static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral)
{

    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+0.1;

    }
    return xvectorPP;
}

static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly)
{
    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 = 0.1;
    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);
    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)
{
    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.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+ 0.1;

//        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 "<<s<<std::endl;
    }
    return xvectorPP;
}

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;

    }
    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)
{
    double frtn = 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(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)
{
    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;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 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(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;
}

//暂不考虑多个LaneSection的情况，不考虑车道宽度变化
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<(nchange1- nchangepoint/2);i++)
        {

            PlanPoint pp = xvPP.at(i);
//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);

            off1 = getoff(xps.mpRoad,nlane1,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.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);

        }
        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);
            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
            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);

            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);

            pp.mfDisToRoadLeft = offx*(-1);
            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);

            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
            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.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
            pp.mfDisToLaneLeft = pp.mWidth/2.0;
            pp.lanmp = 0;
            pp.mfDisToRoadLeft = off2*(-1);
            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);

            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
            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);

            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);

            pp.mfDisToRoadLeft = offx*(-1);
            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);
            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
            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.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
            pp.mfDisToLaneLeft = pp.mWidth/2.0;
            pp.lanmp = 0;
            pp.mfDisToRoadLeft = off3*(-1);
            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);


            xvectorPP.push_back(pp);

        }

    }
    else
    {
        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.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;
        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)
{

  std::vector<PlanPoint> xvectorPPs;

  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);

  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
  int i;
  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);
      xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
//      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++)
      {
          xvectorPPs.push_back(xvPP.at(j));
      }
  }
  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);
        }
    }
}


#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;
//    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);

    if(nfind < 0)return -1;

    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);


    if(nfind_obj < 0)return -2;


    //计算终点在道路的左侧还是右侧
    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))
    {
        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(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<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end);
    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);

    xPlan = xvectorPP;

    }

    int i;

//    QFile xFile;
//    xFile.setFileName("/home/yuchuli/tempgps.txt");
//    xFile.open(QIODevice::ReadWrite);

//    for(i<0;i<xPlan.size();i++)
//    {
//        char strout[1000];
//        snprintf(strout,1000,"%d %f %f %d %d  %d %f \n",i,xPlan[i].mfDisToLaneLeft,
//                 xPlan[i].mfDisToRoadLeft,xPlan[i].mnLaneori,xPlan[i].mnLaneTotal,
//                 xPlan[i].lanmp,xPlan[i].mWidth);
//        xFile.write(strout,strnlen(strout,1000));
//    }
//    xFile.close();

    checktrace(xPlan);
//    std::cout<<"pp size is "<<xvectorPP.size()<<std::endl;
    return 0;

    if(nfind_obj < 0)return -2;

    if(pRoad != pRoad_obj)return -3; //temp,simple plan

    if(pgeob != pgeob_obj)return -4; //temp,simple geo

    if(!pgeob->CheckIfLine())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;i<nlane;i++)
    {
        bianxiandis = bianxiandis + pRoad->GetLaneSection(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);i<pRoad->GetLaneSection(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;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 1;
    indexp = gTempPP.size();


    if(pRoad->GetLaneSection(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;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 0;
    indexp = gTempPP.size();

    double vx = 3.0;
    for(i= (gTempPP.size() -1);i>0;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<gTempPP.size();i++)gTempPP.at(i).lanmp = -1;
    indexp = gTempPP.size();

    double parkx_ext = park_x + 30 * cos(nearhead);
    double parky_ext = park_y + 30 * sin(nearhead);
    speedvalue = 0.0;
    memcpy(q0,q1,3*sizeof(double));
    q1[0]=parkx_ext; q1[1]=parky_ext; q1[2]=nearhead;
    dubins_shortest_path( &path, q0, q1, turning_radius);
    dubins_path_sample_many( &path, 0.1, getPoint, &speedvalue);

    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 0;
    indexp = gTempPP.size();

    for(i=0;i<gTempPP.size();i++)
    {
        xPlan.push_back(gTempPP.at(i));
    }
    return 0;


    PlanPoint pp;
    double hdgsrc,hdgdst;
    double ldis1 = sqrt(pow(xl1 -x_now,2) + pow(yl1 -y_now,2) );
    if(ldis1 > 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);
    }
}