change for localplan.

src/common/common/xodr/OpenDrive/Lane.cpp

@@ -295,7 +295,7 @@ double LaneSection::GetLeftRoadWidth(double s_check)
         pLane = &mLaneVector[i];
         if(pLane->GetId()>0)
         {
-            fwidth = fwidth + pLane->GetWidthValue(s_check);
+            fwidth = fwidth + pLane->GetWidthValue(s_check );
         }
     }
     return fwidth;
@@ -317,7 +317,7 @@ double LaneSection::GetRightRoadWidth(double s_check)
         pLane = &mLaneVector[i];
         if(pLane->GetId()<0)
         {
-            fwidth = fwidth + pLane->GetWidthValue(s_check);
+            fwidth = fwidth + pLane->GetWidthValue(s_check );
         }
     }
     return fwidth;
@@ -502,6 +502,7 @@ Lane * LaneSection::GetRightLaneAt(unsigned int index)
     return pRtn;
 }
 
+
 /**
 * Destructor. Delete all the members of the vectors: mLeft, mCenter, mRight
 */
@@ -528,6 +529,11 @@ string LaneSection::GetSingleSide()
 {
     return msingleSide;
 }
+
+
+
+
+
 /**
 * Lane Section Sample. Holds all the lane information at a certain S value including lane widths, levels, 
 * heights, etc

src/common/common/xodr/OpenDrive/Lane.h

@@ -200,6 +200,9 @@ public:
 
     Lane * GetLeftLaneAt(unsigned int index);
     Lane * GetRightLaneAt(unsigned int index);
+
+
+
 };
 
 

src/common/common/xodr/OpenDrive/Road.cpp

@@ -3,7 +3,7 @@
 #include <math.h>
 
 
-
+#include <iostream>
 
 //***********************************************************************************
 //Road segment
@@ -289,7 +289,7 @@ LaneSection*	Road::GetLaneSection(unsigned int i)
 }
 unsigned int Road::GetLaneSectionCount()
 {
-	return mLaneSectionsVector.size();
+    return static_cast<unsigned int>(mLaneSectionsVector.size()) ;
 }
 
 // Road lane offset records
@@ -1821,6 +1821,162 @@ void  Road::FillLaneSectionSample(double s_check, LaneSectionSample& laneSection
 
 //-------------------------------------------------
 
+std::vector<double> Road::GetDrivingLaneWidthVector(double s_check,int nlr)
+{
+    std::vector<double> xrtn;
+    xrtn.clear();
+
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  xrtn;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return xrtn;
+
+    if(nlr == 1)
+    {
+        unsigned int nlanecount = pLS->GetLeftLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+
+
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+            if(pLane->GetType() != "driving")
+            {
+                break;
+            }
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+    else
+    {
+        unsigned int nlanecount = pLS->GetRightLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetRightLaneAt(i+1);
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+            if(pLane->GetType() != "driving")
+            {
+                break;
+            }
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+
+
+
+    return  xrtn;
+}
+//-------------------------------------------------
+
+int Road::GetLeftDrivingLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    int nrtn;
+
+    unsigned int nlanecount = pLS->GetLeftLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+
+
+        if(pLane == NULL)
+        {
+            return nrtn;
+        }
+        if(pLane->GetType() != "driving")
+        {
+            break;
+        }
+        nrtn++;
+
+
+    }
+
+    return nrtn;
+}
+
+//-------------------------------------------------
+
+int Road::GetRightDrivingLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    int nrtn;
+
+    unsigned int nlanecount = pLS->GetLeftLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetRightLaneAt(i+1);
+
+
+
+        if(pLane == NULL)
+        {
+            return nrtn;
+        }
+        if(pLane->GetType() != "driving")
+        {
+            break;
+        }
+        nrtn++;
+
+
+    }
+
+    return nrtn;
+}
+
+//-------------------------------------------------
+
 /**
  * Destructor
  */

src/common/common/xodr/OpenDrive/Road.h

@@ -442,6 +442,12 @@ public:
     int CheckSignalInterval(double s_check);
 
     int CheckSurfaceCRGInterval(double s_check);
+
+
+    std::vector<double> GetDrivingLaneWidthVector(double s_check,int nlr); //Lane Width, From Refline to side. nlr 1 left 2 right
+
+    int GetLeftDrivingLaneCount(double s_check);
+    int GetRightDrivingLaneCount(double s_check);
 	
 	//-------------------------------------------------
 

src/decition/decition_brain_localplan/brain_localplan_gpstype.cpp

@@ -151,6 +151,7 @@ void brain_localplan_gpstype::UpadateMap(const char *strdata, const unsigned int
         mmutexmapglobal.unlock();
         mbHaveMap = true;
         mbMapUpdate = true;
+        std::cout<<"Update Map."<<std::endl;
     }
     else
     {

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -27,7 +27,8 @@ SOURCES += main.cpp     \
     dubins.c \
     gnss_coordinate_convert.cpp \
     service_roi_xodr.cpp \
-    xodrplan.cpp
+    xodrplan.cpp \
+    ../../include/msgtype/mapglobal.pb.cc
 
 HEADERS += \
     ../../../include/ivexit.h \
@@ -38,7 +39,8 @@ HEADERS += \
     gnss_coordinate_convert.h \
     planpoint.h \
     service_roi_xodr.h \
-    xodrplan.h
+    xodrplan.h \
+    ../../include/msgtype/mapglobal.pb.h
 
 
 !include(../../../include/common.pri ) {

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1405,6 +1405,8 @@ std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
 
             pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);
 
+
+
             if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
             {
                 if(s_start > prg->GetS())
@@ -1490,6 +1492,12 @@ std::vector<PlanPoint> GetPoint(Road * pRoad)
         s = s+ s1;
 
     }
+
+    unsigned int j;
+    for(j=0;j<xvectorPPS.size();j++)
+    {
+        xvectorPPS.at(j).mfCurvature = pRoad->GetRoadCurvature(xvectorPPS.at(j).mS);
+    }
     return xvectorPPS;
 
 }
@@ -2959,6 +2967,9 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                             pp.lanmp = 0;
                             pp.mfDisToRoadLeft = foff *(-1.0);
                             pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mLaneCur = abs(nlane);
+                            pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
+                            pp.mLaneCount = pRoad->GetRightDrivingLaneCount(pp.mS);
                             GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
 
                             xvectorPP.push_back(pp);
@@ -2977,6 +2988,9 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                             pp.lanmp = 0;
                             pp.mfDisToRoadLeft = foff;
                             pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mLaneCur = abs(nlane);
+                            pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
+                            pp.mLaneCount = pRoad->GetLeftDrivingLaneCount(pp.mS);
                             GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
 
 

src/driver/driver_map_xodrload/planpoint.h

@@ -1,6 +1,9 @@
 #ifndef PLANPOINT_H
 #define PLANPOINT_H
 
+
+#include <vector>
+
 class PlanPoint
 {
 public:
@@ -36,6 +39,10 @@ public:
     bool mbBoringRoad = false;
     bool mbNoavoid = false;
     double mfCurvature = 0.0;
+
+    int mLaneCur;  //Current Lane 1 is left
+    int mLaneCount; //Current Side Lane
+    std::vector<double> mVectorLaneWidth; //Lane Width
 };
 
 #endif // PLANPOINT_H