change driver/driver_map_xodrload for add signal support.

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -19,19 +19,19 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += main.cpp     \
     ../../include/msgtype/v2x.pb.cc \
-OpenDrive/OpenDriveXmlWriter.cpp \
-OpenDrive/OtherStructures.cpp \
-OpenDrive/OpenDrive.cpp \
-OpenDrive/Road.cpp \
-OpenDrive/Junction.cpp \
-OpenDrive/Lane.cpp \
-OpenDrive/OpenDriveXmlParser.cpp \
-OpenDrive/ObjectSignal.cpp \
-OpenDrive/RoadGeometry.cpp \
-TinyXML/tinyxml.cpp \
-TinyXML/tinyxmlparser.cpp \
-TinyXML/tinystr.cpp \
-TinyXML/tinyxmlerror.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp \
+    ../../common/common/xodr/OpenDrive/OtherStructures.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDrive.cpp \
+    ../../common/common/xodr/OpenDrive/Road.cpp \
+    ../../common/common/xodr/OpenDrive/Junction.cpp \
+    ../../common/common/xodr/OpenDrive/Lane.cpp \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp \
+    ../../common/common/xodr/OpenDrive/ObjectSignal.cpp \
+    ../../common/common/xodr/OpenDrive/RoadGeometry.cpp \
+    ../../common/common/xodr/TinyXML/tinyxml.cpp \
+    ../../common/common/xodr/TinyXML/tinyxmlparser.cpp \
+    ../../common/common/xodr/TinyXML/tinystr.cpp \
+    ../../common/common/xodr/TinyXML/tinyxmlerror.cpp \
     fresnl.cpp \
     const.cpp \
     polevl.c \
@@ -46,17 +46,17 @@ TinyXML/tinyxmlerror.cpp \
 HEADERS += \
     ../../../include/ivexit.h \
     ../../include/msgtype/v2x.pb.h \
-OpenDrive/OpenDriveXmlParser.h \
-OpenDrive/RoadGeometry.h \
-OpenDrive/Road.h \
-OpenDrive/Junction.h \
-OpenDrive/OpenDriveXmlWriter.h \
-OpenDrive/Lane.h \
-OpenDrive/OtherStructures.h \
-OpenDrive/OpenDrive.h \
-OpenDrive/ObjectSignal.h \
-TinyXML/tinyxml.h \
-TinyXML/tinystr.h \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlParser.h \
+    ../../common/common/xodr/OpenDrive/RoadGeometry.h \
+    ../../common/common/xodr/OpenDrive/Road.h \
+    ../../common/common/xodr/OpenDrive/Junction.h \
+    ../../common/common/xodr/OpenDrive/OpenDriveXmlWriter.h \
+    ../../common/common/xodr/OpenDrive/Lane.h \
+    ../../common/common/xodr/OpenDrive/OtherStructures.h \
+    ../../common/common/xodr/OpenDrive/OpenDrive.h \
+    ../../common/common/xodr/OpenDrive/ObjectSignal.h \
+    ../../common/common/xodr/TinyXML/tinyxml.h \
+    ../../common/common/xodr/TinyXML/tinystr.h \
     mconf.h \
     globalplan.h \
     gps_type.h \
@@ -84,4 +84,6 @@ TinyXML/tinystr.h \
     error( "Couldn't find the iveigen.pri file!" )
 }
 
+INCLUDEPATH += $$PWD/../../common/common/xodr
+
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -19,6 +19,8 @@ extern "C"
 #include "dubins.h"
 }
 
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
 /**
  * @brief GetLineDis 获得点到直线Geometry的距离。
  * @param pline
@@ -1916,7 +1918,9 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
         }
     }
 
-    Road * pRoad = xps.mpRoad;
+
+
+
 
 
     if(xps.mnStartLaneSel > 0)
@@ -1927,14 +1931,64 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
         {
             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,
@@ -1949,6 +2003,7 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
   int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
   int i;
   std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP);
+
   if(xpathsection[0].mainsel < 0)
   {
   for(i=indexstart;i<xvPP.size();i++)
@@ -2255,6 +2310,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
         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;

src/driver/driver_map_xodrload/globalplan.h

@@ -24,6 +24,8 @@ public:
     double mfDisToLaneLeft;
     int mnLaneori = 0;  //from Right 0
     int mnLaneTotal = 1; //Lane Total
+
+    int nSignal = -1;   //if 0 no signal point
 };
 
 int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,

src/driver/driver_map_xodrload/main.cpp

@@ -467,12 +467,30 @@ void SetPlan(xodrobj xo)
         givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
 
-        data->roadSum = 1;
-        data->roadMode = 0;
-        data->roadOri = 0;
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
 
-        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                mapdata.at(k)->mode2 = 3000;
+            }
+        }
+
+//        data->roadSum = 1;
+//        data->roadMode = 0;
+//        data->roadOri = 0;
+
+//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
 
         mapdata.push_back(data);