change driver_map_xodrload. change plan.

src/driver/driver_map_xodrload/globalplan.cpp

@@ -686,23 +686,27 @@ int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoa
 }
 
 
-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)
+namespace iv {
+struct nearroad
 {
+Road * pObjRoad;
+GeometryBlock * pgeob;
+double nearx;
+double neary;
+double nearhead;
+double frels;
+double fdis;
+int lr = 2; //1 left 2 right;
+int nmode = 0; //0 same direciion dis zero   1 oposite dis = 0 2 same direction dis<5 3 oposite  dis <5   4 same direction > 5 5 oposite direction;
+};
+}
 
-    double dismin = std::numeric_limits<double>::infinity();
-    s = dismin;
+int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
+                         const double nearthresh,bool bhdgvalid = true)
+{
     int i;
     double frels;
-    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;
-    std::vector<double > xvectorfrels;
-    double VECTORTHRESH = 5;
+    int nminmode = 5;
     for(i=0;i<pxodr->GetRoadCount();i++)
     {
         int j;
@@ -721,6 +725,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
 
         for(j=0;j<proad->GetGeometryBlockCount();j++)
         {
+            localdismin = std::numeric_limits<double>::infinity();
             GeometryBlock * pgb = proad->GetGeometryBlock(j);
             double dis;
             RoadGeometry * pg;
@@ -748,21 +753,8 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
                 break;
             }
 
-            if(dis < dismin)
-            {
-                dismin = dis;
-                nearx = nx;
-                neary = ny;
-                nearhead = nh;
-                s = dis;
-                froads = frels;
-   //             qDebug("rels s is %f ",frels);
-                *pObjRoad = proad;
-                *pgeo = pgb;
 
-            }
-
-            if((dis<VECTORTHRESH) &&(dis<localdismin))
+            if(dis<localdismin)
             {
                 localdismin = dis;
                 localnearx = nx;
@@ -775,98 +767,185 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
             }
 
 
-
-
         }
 
-        if(bchange)
+        if(localdismin < nearthresh)
         {
-            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);
-            xvectorfrels.push_back(localfrels);
-        }
-    }
-
+            iv::nearroad xnear;
+            xnear.pObjRoad = proad;
+            xnear.pgeob = pgeolocal;
+            xnear.nearx = localnearx;
+            xnear.neary = localneary;
+            xnear.fdis = localdismin;
+            xnear.nearhead = localnearhead;
+            xnear.frels = localfrels;
+            if(xnear.fdis>0)
+            {
+                double fcalhdg = xodrfunc::CalcHdg(QPointF(xnear.nearx,xnear.neary),QPointF(x,y));
+                double fhdgdiff = fcalhdg - xnear.nearhead;
+                while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
+                while(fhdgdiff>= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
+                if(fhdgdiff<M_PI)
+                {
+                    double fdisroad = proad->GetRoadLeftWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 1;
+                }
+                else
+                {
+                    double fdisroad = proad->GetRoadRightWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 2;
+                }
 
-    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))
+                if(bhdgvalid == false)
                 {
-                    return 0;
+                    if(xnear.pObjRoad->GetLaneSectionCount()>0)
+                    {
+                        LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+                        if(pLS->GetRightLaneCount()>0)
+                        {
+                            xnear.lr = 2;
+                        }
+                        else
+                        {
+                            xnear.lr = 1;
+                        }
+                    }
+                    else
+                    {
+                        xnear.lr = 2;
+                    }
+                }
+            }
+            LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+            if(pLS != NULL)
+            {
+            if(xnear.fdis < 0.00001)
+            {
+                if(bhdgvalid == false)
+                {
+                    xnear.nmode = 0;
                 }
                 else
                 {
-                    bool bHaveSame = false;
-                    for(i=0;i<xvectorroad.size();i++)
+                    double headdiff = hdg - xnear.nearhead;
+                    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                    if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                    {
+                       xnear.nmode = 0;
+                    }
+                    else
+                    {
+                        xnear.nmode = 1;
+                    }
+                }
+
+            }
+            else
+            {
+                if(xnear.fdis<5)
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 2;
+                    }
+                    else
                     {
-                        if(xvectorroad[i]->GetLaneSectionCount()<1)continue;
-                        bool bNeedFind = false;
-                        if(bHaveSame == false)bNeedFind = true;
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                        {
+                            xnear.nmode = 2;
+                        }
                         else
                         {
-                            if(xvectordismin[i]<dismin)bNeedFind = true;
+                            xnear.nmode = 3;
                         }
-                        if(bNeedFind)
+                    }
+
+                }
+                else
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 4;
+                    }
+                    else
+                    {
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
                         {
-                            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];
-                                froads = xvectorfrels[i];
-                                *pObjRoad = xvectorroad[i];
-                                *pgeo = xvectorgeob[i];
-                                bHaveSame = true;
-                                std::cout<<"change road. "<<std::endl;
-                            }
+                            xnear.nmode = 4;
+                        }
+                        else
+                        {
+                            xnear.nmode = 5;
                         }
                     }
                 }
             }
+            if(xnear.nmode < nminmode)nminmode = xnear.nmode;
+            xvectornear.push_back(xnear);
+            }
         }
+
     }
 
+    if(xvectornear.size() == 0)return -1;
+
+
+    if(xvectornear.size() > 1)
+    {
+        int i;
+        for(i=0;i<(int)xvectornear.size();i++)
+        {
+            if(xvectornear[i].nmode > nminmode)
+            {
+                xvectornear.erase(xvectornear.begin() + i);
+                i = i-1;
+            }
+        }
+    }
+    if((xvectornear.size()>1)&&(nminmode>=4))  //if dis > 5 select small dis
+    {
+        int i;
+        while(xvectornear.size()>1)
+        {
+            if(xvectornear[1].fdis < xvectornear[0].fdis)
+            {
+                xvectornear.erase(xvectornear.begin());
+            }
+            else
+            {
+                xvectornear.erase(xvectornear.begin()+1);
+            }
+        }
+    }
     return 0;
+
 }
 
 
@@ -2760,420 +2839,190 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
     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();
-
+    Road * pRoad_obj;GeometryBlock * pgeob_obj;
     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);
-
+    int lr_end = 2;
+    int lr_start = 2;
+    double fs1,fs2;
+//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
 
-    if(nfind_obj < 0)return -2;
-    fs2 = fs2 + pgeob_obj->GetGeometryAt(0)->GetS();
+    std::vector<iv::nearroad> xvectornearstart;
+    std::vector<iv::nearroad> xvectornearend;
 
+    GetNearPointWithHead(x_now,y_now,head,pxodr,xvectornearstart,srcnearthresh,true);
+    GetNearPointWithHead(x_obj,y_obj,0,pxodr,xvectornearend,dstnearthresh,false);
 
-    //计算终点在道路的左侧还是右侧
-    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
+    if(xvectornearstart.size()<1)
     {
-        lr_end = 1;
-    }
-
-    if((pRoad_obj->GetLaneSection(0)->GetLeftLaneCount()<1)&&(lr_end == 1))
-    {
-        lr_end = 2;
+        std::cout<<" plan fail. can't find start point."<<std::endl;
+        return -1;
     }
-
-    if((pRoad_obj->GetLaneSection(0)->GetRightLaneCount()<1)&&(lr_end == 2))
+    if(xvectornearend.size() < 1)
     {
-        lr_end = 2;
+        std::cout<<" plan fail. can't find end point."<<std::endl;
+        return -2;
     }
 
-    int lr_start = SelectRoadLeftRight(pRoad,head,nearhead);
+    std::vector<std::vector<PlanPoint>> xvectorplans;
+    std::vector<double> xvectorroutedis;
 
-    bool bNeedDikstra = true;
-    if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
+    int i;
+    int j;
+    for(i=0;i<(int)xvectornearstart.size();i++)
     {
-        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)
+        for(j=0;j<(int)xvectornearend.size();j++)
         {
+            iv::nearroad * pnearstart = &xvectornearstart[i];
+            iv::nearroad * pnearend = &xvectornearend[j];
+            pRoad = pnearstart->pObjRoad;
+            pgeob = pnearstart->pgeob;
+            pRoad_obj = pnearend->pObjRoad;
+            pgeob_obj = pnearend->pgeob;
+            lr_start = pnearstart->lr;
+            lr_end = pnearend->lr;
 
-            std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
-            std::vector<int> xvectorindex1;
+            nearx = pnearstart->nearx;
+            neary = pnearstart->neary;
 
-            xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
+            nearx_obj = pnearend->nearx;
+            neary_obj = pnearend->neary;
 
-            double foff = getoff(pRoad,nlane);
-
-            foff = foff + 0.0;
-            std::vector<PlanPoint> xvectorPP;
-            int i = nindexstart;
-            while(i!=nindexend)
+            bool bNeedDikstra = true;
+            if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
             {
-
-                if(lr_start == 2)
+                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))
                 {
-                    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,pp.mbNoavoid);
-
-                    xvectorPP.push_back(pp);
-                    i++;
+                    bNeedDikstra = false;
                 }
-                else
+                if((nindexend <nindexstart)&&(lr_start == 1))
                 {
-                    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,pp.mbNoavoid);
-
-
-                    xvectorPP.push_back(pp);
-                    i--;
+                    bNeedDikstra = false;
                 }
-            }
-
-            for(i=0;i<(int)xvectorPP.size();i++)
-            {
-                xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
-            }
-
-            pathsection xps;
-            xps.mbStartToMainChange = false;
-            xps.mbMainToEndChange = false;
- //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
-            xPlan = xvectorPP;
-
-        }
-    }
-
-    if(bNeedDikstra)
-    {
-        bool bSuc = false;
-    std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
-    if(xpath.size()<1)return -1;
-    if(bSuc == false)
-    {
-        return -1;
-    }
-    double flen = pxd->getpathlength(xpath);
-    std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
-
-    std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
-                 head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
-
-    ChangeLane(xvectorPP);
-    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);
+                if(bNeedDikstra == false)
+                {
 
-    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 1;
-    indexp = gTempPP.size();
+                    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
+                    std::vector<int> xvectorindex1;
 
+                    xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
 
-    if(pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeedCount()>1)
-        speedvalue = pRoad->GetLaneSection(0)->GetLane(nlane)->GetLaneSpeed(1)->GetMax();
-    else speedvalue = 10.0;
+                    double foff = getoff(pRoad,nlane);
 
-    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);
+                    foff = foff + 0.0;
+                    std::vector<PlanPoint> xvectorPP;
+                    int i = nindexstart;
+                    while(i!=nindexend)
+                    {
 
-    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = 0;
-    indexp = gTempPP.size();
+                        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,pp.mbNoavoid);
+
+                            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,pp.mbNoavoid);
+
+
+                            xvectorPP.push_back(pp);
+                            i--;
+                        }
+                    }
 
-    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;
-    }
+                    for(i=0;i<(int)xvectorPP.size();i++)
+                    {
+                        xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
+                    }
 
-    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);
+                    pathsection xps;
+                    xps.mbStartToMainChange = false;
+                    xps.mbMainToEndChange = false;
+         //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
+                    xPlan = xvectorPP;
 
-    for(i=indexp;i<gTempPP.size();i++)gTempPP.at(i).lanmp = -1;
-    indexp = gTempPP.size();
+                    xvectorplans.push_back(xvectorPP);
+                    xvectorroutedis.push_back(xvectorPP.size() * 0.1);
 
-    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();
+            if(bNeedDikstra)
+            {
+                bool bSuc = false;
+            std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
+            if(xpath.size()<1)
+            {
+                continue;
+            }
+            if(bSuc == false)
+            {
+                continue;
+            }
+            double flen = pxd->getpathlength(xpath);
+            std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
 
-    for(i=0;i<gTempPP.size();i++)
-    {
-        xPlan.push_back(gTempPP.at(i));
-    }
-    return 0;
+            std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
+                         head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
 
+            ChangeLane(xvectorPP);
+            xvectorplans.push_back(xvectorPP);
+            xvectorroutedis.push_back(flen);
 
-    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)
+    if(xvectorplans.size() > 0)
     {
-        while((disx+step) < ldis1)
+        if(xvectorplans.size() == 1)
         {
-            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);
+            xPlan = xvectorplans[0];
         }
-    }
-
-    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)
+        else
         {
-            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
+            int nsel = 0;
+            double fdismin = xvectorroutedis[0];
+            for(i=1;i<(int)xvectorroutedis.size();i++)
             {
-                pp.lanmp = 0;
+                if(xvectorroutedis[i]<fdismin)
+                {
+                    nsel = i;
+                }
             }
-            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);
-
+            xPlan = xvectorplans[nsel];
         }
+        return 0;
     }
+    std::cout<<" plan fail. can't find path."<<std::endl;
+    return -1000;
 
-    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);
-    }
 }

src/tool/map_collectfromveh/collectconvert.cpp

@@ -39,7 +39,12 @@ void CollectConvert::threadconvert()
 
 void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxodr)
 {
-    if(proad->mpoints_size() == 0)return;
+    if(proad->mpoints_size() < 2)
+    {
+        std::cout<<" road : "<<proad->strroadname()<<" only "
+                <<proad->mpoints_size()<<" points. need least 2 points."<<std::endl;
+        return;
+    }
     double flon0,flat0;
     pxodr->GetHeader()->GetLat0Lon0(flat0,flon0);
 //    pxodr->GetHeader()->GetLat0Lon0(flon0,flat0);

src/tool/map_collectfromveh/map_collectfromveh.pro

@@ -16,11 +16,11 @@ SOURCES += \
     ../map_lanetoxodr/TinyXML/tinyxml.cpp \
     ../map_lanetoxodr/TinyXML/tinyxmlerror.cpp \
     ../map_lanetoxodr/TinyXML/tinyxmlparser.cpp \
-    ../map_lanetoxodr/autoconnect.cpp \
-    ../map_lanetoxodr/circlefitting.cpp \
+    ../map_lanetoxodr/function/autoconnect.cpp \
+    ../map_lanetoxodr/function/circlefitting.cpp \
     ../map_lanetoxodr/const.cpp \
     ../map_lanetoxodr/fresnl.cpp \
-    ../map_lanetoxodr/geofit.cpp \
+    ../map_lanetoxodr/function/geofit.cpp \
     ../map_lanetoxodr/polevl.c \
     collectconvert.cpp \
     commif.cpp \
@@ -37,9 +37,9 @@ HEADERS += \
     ../../include/msgtype/gpsimu.pb.h \
     ../map_lanetoxodr/TinyXML/tinystr.h \
     ../map_lanetoxodr/TinyXML/tinyxml.h \
-    ../map_lanetoxodr/autoconnect.h \
-    ../map_lanetoxodr/circlefitting.h \
-    ../map_lanetoxodr/geofit.h \
+    ../map_lanetoxodr/function/autoconnect.h \
+    ../map_lanetoxodr/function/circlefitting.h \
+    ../map_lanetoxodr/function/geofit.h \
     collectconvert.h \
     commif.h \
     dialogconvert.h \
@@ -83,6 +83,7 @@ INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
 INCLUDEPATH += ../../common/common/math
 
 INCLUDEPATH += ../map_lanetoxodr
+INCLUDEPATH += ../map_lanetoxodr/function
 
 unix:INCLUDEPATH += /usr/include/eigen3
 win32:INCLUDEPATH += D:\File\soft\eigen