change driver/driver_map_xodrload. adding roi service.

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

@@ -279,6 +279,50 @@ double LaneSection::GetS2()
 
 }
 
+
+/**
+ * @brief LaneSection::GetLeftRoadWidth
+ * @return Road Width of Left Side
+ */
+double LaneSection::GetLeftRoadWidth(double s_check)
+{
+    Lane * pLane;
+    unsigned int nLaneCount = mLaneVector.size();
+    unsigned int i;
+    double fwidth = 0;
+    for(i=0;i<nLaneCount;i++)
+    {
+        pLane = &mLaneVector[i];
+        if(pLane->GetId()>0)
+        {
+            fwidth = fwidth + pLane->GetWidthValue(s_check);
+        }
+    }
+    return fwidth;
+}
+
+/**
+ * @brief LaneSection::GetRightRoadWidth
+ * @param s_check
+ * @return  Road Width of Right Side
+ */
+double LaneSection::GetRightRoadWidth(double s_check)
+{
+    Lane * pLane;
+    unsigned int nLaneCount = mLaneVector.size();
+    unsigned int i;
+    double fwidth = 0;
+    for(i=0;i<nLaneCount;i++)
+    {
+        pLane = &mLaneVector[i];
+        if(pLane->GetId()<0)
+        {
+            fwidth = fwidth + pLane->GetWidthValue(s_check);
+        }
+    }
+    return fwidth;
+}
+
 /**
 * Set the lane section s-offset
 */

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

@@ -130,6 +130,19 @@ public:
 	*/
 	double GetS2();
 
+    /**
+     * @brief GetLeftRoadWidth Get Road Width of Left Side
+     * @return Road Width of left side
+     */
+    double GetLeftRoadWidth(double s_check);
+
+    /**
+     * @brief GetRightRoadWidth
+     * @param s_check
+     * @return Road Width of right side
+     */
+    double GetRightRoadWidth(double s_check);
+
 	/**
 	* Set the lane section s-offset
 	*/

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

@@ -1095,6 +1095,47 @@ int Road::GetRoadSpeedMax(double s_check, double &fspeed)
     }
     return 0;
 }
+
+double Road::GetRoadLeftWidth(double s_check)
+{
+    if(mLaneSectionsVector.size() == 0)
+        return 0.0;
+    if(s_check > mLength)return 0.0;
+    if(s_check < 0)return 0.0;
+    LaneSection * pLS =&mLaneSectionsVector[0];
+    if(mLaneSectionsVector.size() > 1)
+    {
+        unsigned int nLSCount = mLaneSectionsVector.size();
+        unsigned int i;
+        for(i=0;i<(nLSCount -1);i++)
+        {
+            if(pLS->GetS()>s_check)break;
+            pLS = &mLaneSectionsVector[i+1];
+        }
+    }
+    return pLS->GetLeftRoadWidth(s_check);
+}
+
+double Road::GetRoadRightWidth(double s_check)
+{
+    if(mLaneSectionsVector.size() == 0)
+        return 0.0;
+    if(s_check > mLength)return 0.0;
+    if(s_check < 0)return 0.0;
+    LaneSection * pLS =&mLaneSectionsVector[0];
+    if(mLaneSectionsVector.size() > 1)
+    {
+        unsigned int nLSCount = mLaneSectionsVector.size();
+        unsigned int i;
+        for(i=0;i<(nLSCount -1);i++)
+        {
+            if(pLS->GetS()>s_check)break;
+            pLS = &mLaneSectionsVector[i+1];
+        }
+    }
+    return pLS->GetRightRoadWidth(s_check);
+}
+
 //-----------
 
 int Road::GetRoadNoavoid(double s_check, bool &bNoavoid)

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

@@ -322,6 +322,9 @@ public:
     int GetRoadSpeedMax(double s_check,double & fspeed);
     int GetRoadNoavoid(double s_check,bool & bNoavoid);
 
+    double GetRoadLeftWidth(double s_check);
+    double GetRoadRightWidth(double s_check);
+
 	/**
 	 * Other evaluation
 	 */

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -20,6 +20,8 @@ QMAKE_CXXFLAGS +=  -g
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp     \
+    ../../include/msgtype/roireqest.pb.cc \
+    ../../include/msgtype/roiresult.pb.cc \
     ../../include/msgtype/v2x.pb.cc \
     fresnl.cpp \
     const.cpp \
@@ -30,11 +32,14 @@ SOURCES += main.cpp     \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/imu.pb.cc \
     gnss_coordinate_convert.cpp \
+    service_roi_xodr.cpp \
     xodrplan.cpp \
     ../../include/msgtype/mapdata.pb.cc
 
 HEADERS += \
     ../../../include/ivexit.h \
+    ../../include/msgtype/roireqest.pb.h \
+    ../../include/msgtype/roiresult.pb.h \
     ../../include/msgtype/v2x.pb.h \
     mconf.h \
     globalplan.h \
@@ -45,6 +50,7 @@ HEADERS += \
     ../../include/msgtype/imu.pb.h \
     gnss_coordinate_convert.h \
     planpoint.h \
+    service_roi_xodr.h \
     xodrplan.h \
     ../../include/msgtype/mapdata.pb.h
 

src/driver/driver_map_xodrload/main.cpp

@@ -34,6 +34,8 @@
 
 #include <signal.h>
 
+#include "service_roi_xodr.h"
+
 OpenDrive mxodr;
 xodrdijkstra * gpxd;
 bool gmapload = false;
@@ -138,6 +140,9 @@ bool LoadXODR(std::string strpath)
     Road * proad1 = mxodr.GetRoad(0);
     givlog->info("road name is %s",proad1->GetRoadName().data());
     std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
+
+    service_roi_xodr * psrx = new service_roi_xodr();
+    psrx->SetXODR(&mxodr);
 }
 
 class roadx

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -0,0 +1,83 @@
+#include "service_roi_xodr.h"
+
+#include <iostream>
+#include "gnss_coordinate_convert.h"
+
+service_roi_xodr::service_roi_xodr()
+{
+
+}
+
+void service_roi_xodr::SetXODR(OpenDrive *pxodr)
+{
+    mpxodr = pxodr;
+    updateroadarea();
+}
+
+int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_prt)
+{
+
+    return 0;
+}
+
+void service_roi_xodr::updateroadarea()
+{
+    OpenDrive * pxodr = mpxodr;
+    mvectorarea.clear();
+    unsigned int nroadcount = pxodr->GetRoadCount();
+    unsigned int i;
+    const double fstep = 0.1;
+    for(i=0;i<nroadcount;i++)
+    {
+        Road * pRoad = pxodr->GetRoad(i);
+        if(pRoad == NULL)
+        {
+            std::cout<<" Warning: OpenDrive Road "<<i<<" is NULL"<<std::endl;
+            continue;
+        }
+        iv::roi::roadarea xroadarea = GetRoadArea(pRoad,fstep);
+        mvectorarea.push_back(xroadarea);
+    }
+}
+
+iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
+{
+    iv::roi::roadarea xroadarea;
+    xroadarea.fRoadLen = pRoad->GetRoadLength();
+    unsigned int nSectionCount = pRoad->GetLaneSectionCount();
+    unsigned int i;
+    if(nSectionCount == 0)
+    {
+        std::cout<<"Warning. Road"<<pRoad->GetRoadId()<<" not have lanesection."<<std::endl;
+    }
+    double snow= 0;
+    for(i=0;i<nSectionCount;i++)
+    {
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(pLS == NULL)
+        {
+            std::cout<<"Warning. LaneSection is NULL."<<std::endl;
+            continue;
+        }
+        snow = pLS->GetS();
+        double endS;
+        if(i == (nSectionCount -1))
+        {
+            endS = pRoad->GetRoadLength();
+        }
+        else
+        {
+            endS = pRoad->GetLaneSection(i+1)->GetS();
+        }
+
+        while(snow < endS)
+        {
+//            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+//            double frigthwidth = pRoad->GetRoadRightWidth(snow);
+//            std::cout<<" road : "<<pRoad->GetRoadId()<<" s: "<<snow<<" left: "<<fleftwidth<<" right:"<<frigthwidth<<std::endl;
+            snow = snow + fstep;
+        }
+    }
+    return xroadarea;
+}
+

src/driver/driver_map_xodrload/service_roi_xodr.h

@@ -0,0 +1,74 @@
+#ifndef SERVICE_ROI_XODR_H
+#define SERVICE_ROI_XODR_H
+
+#include <vector>
+#include <OpenDrive/OpenDrive.h>
+#include "gpsimu.pb.h"
+#include "roireqest.pb.h"
+#include "roiresult.pb.h"
+
+namespace iv {
+
+namespace roi {
+
+struct Point
+{
+    double x;
+    double y;
+};
+
+struct area
+{
+
+    double s;
+    Point refPoint;
+    Point P1;
+    Point P2;
+    Point P3;
+    Point P4;
+
+};
+
+struct roadarea
+{
+    int nroadid;
+    Point startPoint;
+    Point endPoint;
+    double fRoadLen;
+    std::vector<area> mvectorarea;
+};
+}
+}
+
+class service_roi_xodr
+{
+
+
+public:
+    service_roi_xodr();
+
+    /**
+     * @brief SetXODR Set OpenDrive Pointer to Service
+     * @param pxodr
+     */
+    void SetXODR(OpenDrive * pxodr);
+
+    /**
+     * @brief GetROI Get ROI From XODR Map
+     * @param xreq_ptr  now position and roi param
+     * @param xres_prt  result
+     * @return 0 Succeed -1 No Map  -2 Not in Map
+     */
+    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_prt );
+
+private:
+    void updateroadarea();
+    iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
+
+private:
+    OpenDrive * mpxodr = NULL;
+    std::vector<iv::roi::roadarea> mvectorarea;
+
+};
+
+#endif // SERVICE_ROI_XODR_H

src/include/proto/roireqest.proto

@@ -0,0 +1,15 @@
+syntax = "proto2";
+
+package iv.roi;
+
+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 gridsize	= 5;   //grid size,eq,0.2
+};
+
+
+

src/include/proto/roiresult.proto

@@ -0,0 +1,22 @@
+syntax = "proto2";
+
+package iv.roi;
+
+message resultunit
+{
+	required double x		= 1;  // x is front
+	required double	y		= 2;  // y is left
+};
+
+message resultarray
+{
+	required double lat		= 1;
+	required double lon		= 2;
+	required double heading		= 3;
+	required double range		= 4;
+	required double gridsize	= 5;
+	repeated resultunit res		= 6;
+};
+
+
+