change driver_map_xodrload. adding roi service.

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

@@ -1136,6 +1136,26 @@ double Road::GetRoadRightWidth(double s_check)
     return pLS->GetRightRoadWidth(s_check);
 }
 
+double Road::GetLaneOffsetValue(double s_check)
+{
+    if(mLaneOffsetVector.size() == 0)return 0.0;
+    unsigned int noffsetcount = mLaneOffsetVector.size();
+    unsigned int i;
+    LaneOffset * pLO = NULL;
+    for(i=0;i<noffsetcount;i++)
+    {
+        double s = mLaneOffsetVector[i].GetS();
+        if(s>s_check)
+        {
+            break;
+        }
+        pLO = &mLaneOffsetVector[i];
+    }
+    if(pLO == NULL)return 0.0;
+    double stem = s_check - pLO->GetS();
+    return pLO->Geta() + pLO->Getb() * stem + pLO->Getc() * stem * stem + pLO->Getd() * stem * stem * stem;
+}
+
 //-----------
 
 int Road::GetRoadNoavoid(double s_check, bool &bNoavoid)

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

@@ -325,6 +325,8 @@ public:
     double GetRoadLeftWidth(double s_check);
     double GetRoadRightWidth(double s_check);
 
+    double GetLaneOffsetValue(double s_check);
+
 	/**
 	 * Other evaluation
 	 */

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -11,12 +11,62 @@ service_roi_xodr::service_roi_xodr()
 void service_roi_xodr::SetXODR(OpenDrive *pxodr)
 {
     mpxodr = pxodr;
+    mpxodr->GetHeader()->GetLat0Lon0(mlat0,mlon0);
     updateroadarea();
 }
 
 int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_prt)
 {
 
+    double x0,y0;
+    double x,y;
+    GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
+    GaussProjCal(mlon0,mlat0,&x0,&y0);
+    x = x - x0;
+    y = y - y0;
+    unsigned int nroadsize = mvectorarea.size();
+    unsigned int i;
+    std::vector<iv::roi::area> xvectorarea;
+    for(i=0;i<nroadsize;i++)
+    {
+        double fdis1;
+        iv::roi::roadarea * proadarea = &mvectorarea[i];
+        fdis1 = sqrt(pow(proadarea->startPoint.x - x,2)+pow(proadarea->startPoint.y - y,2));
+        if(fdis1 > (xreq_ptr->range() + proadarea->fRoadLen + 30.0))  //+30.0 road width.
+        {
+            continue;
+        }
+        unsigned int nareasize = proadarea->mvectorarea.size();
+        unsigned int j;
+        for(j=0;j<nareasize;j++)
+        {
+            iv::roi::area * parea = &proadarea->mvectorarea[j];
+            double fd1,fd2,fd3,fd4;
+            fd1 = sqrt(pow(x - parea->P1.x,2)+pow(y - parea->P1.y,2));
+            if(fd1<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd2 = sqrt(pow(x - parea->P2.x,2)+pow(y - parea->P2.y,2));
+            if(fd2<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd3 = sqrt(pow(x - parea->P3.x,2)+pow(y - parea->P3.y,2));
+            if(fd3<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd4 = sqrt(pow(x - parea->P4.x,2)+pow(y - parea->P4.y,2));
+            if(fd4<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+            }
+        }
+    }
     return 0;
 }
 
@@ -44,6 +94,10 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
 {
     iv::roi::roadarea xroadarea;
     xroadarea.fRoadLen = pRoad->GetRoadLength();
+    double x1,y1,hdg1;
+    pRoad->GetGeometryCoords(0.0,x1,y1,hdg1);
+    xroadarea.startPoint.x = x1;
+    xroadarea.startPoint.y = y1;
     unsigned int nSectionCount = pRoad->GetLaneSectionCount();
     unsigned int i;
     if(nSectionCount == 0)
@@ -70,12 +124,67 @@ iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
             endS = pRoad->GetLaneSection(i+1)->GetS();
         }
 
+        double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+        double frigthwidth = pRoad->GetRoadRightWidth(snow);
+        double x,y,yaw;
+        pRoad->GetGeometryCoords(snow,x,y,yaw);
+        iv::roi::Point pointLeft,pointRight,pointLeft_old,pointRight_old;
+        double flaneoff = pRoad->GetLaneOffsetValue(snow);
+        pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+        pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+        pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+        pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+        pointLeft_old = pointLeft;
+        pointRight_old = pointRight;
+        snow = snow + fstep;
         while(snow < endS)
         {
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xroadarea.mvectorarea.push_back(xarea);
 //            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
 //            double frigthwidth = pRoad->GetRoadRightWidth(snow);
 //            std::cout<<" road : "<<pRoad->GetRoadId()<<" s: "<<snow<<" left: "<<fleftwidth<<" right:"<<frigthwidth<<std::endl;
+
+            pointLeft_old = pointLeft;
+            pointRight_old = pointRight;
             snow = snow + fstep;
+
+        }
+        if(snow < (endS - 0.000001))
+        {
+            snow = endS - 0.000001;
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xroadarea.mvectorarea.push_back(xarea);
         }
     }
     return xroadarea;

src/driver/driver_map_xodrload/service_roi_xodr.h

@@ -33,7 +33,6 @@ struct roadarea
 {
     int nroadid;
     Point startPoint;
-    Point endPoint;
     double fRoadLen;
     std::vector<area> mvectorarea;
 };
@@ -68,6 +67,8 @@ private:
 private:
     OpenDrive * mpxodr = NULL;
     std::vector<iv::roi::roadarea> mvectorarea;
+    double mlon0;
+    double mlat0;
 
 };
 

src/include/proto/roireqest.proto

@@ -7,7 +7,7 @@ message request
 	required double lat		= 1;
 	required double	lon		= 2;
 	required double heading		= 3;
-	required double range		= 4;   //roi area is (range + range)*(range + range)
+	required double range		= 4;   //circle radius
 	required double gridsize	= 5;   //grid size,eq,0.2
 };