complete optimize road. begin add Road TurnDirection for Autoware.

src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.cpp

@@ -190,6 +190,12 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
         WriteRoadPriority(nodeRoad,road);
     }
 
+    RoadTurnDirection::TURNDIRECTION xturn;
+    if(road->GetRoadTurnDirection(xturn) == 1)
+    {
+        WriteRoadTurnDirection(nodeRoad,road);
+    }
+
     if(road->GetRoadNoavoidCount()>0)
     {
         WriteRoadNoavoids(nodeRoad,road);
@@ -1944,6 +1950,28 @@ bool OpenDriveXmlWriter::WriteRoadPriority(TiXmlElement * node,Road * road)
     return true;
 }
 
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteRoadTurnDirection(TiXmlElement * node, Road * road)
+{
+    TiXmlElement* nodeRoadTurnDirection = new TiXmlElement("userData");
+    nodeRoadTurnDirection->SetAttribute("code","roadTurnDirection");
+    node->LinkEndChild(nodeRoadTurnDirection);
+
+    RoadTurnDirection::TURNDIRECTION xturn;
+    if(road->GetRoadTurnDirection(xturn) == 1)
+    {
+        TiXmlElement* nodeRoadTD = new TiXmlElement("roadTurnDirection");
+        nodeRoadTurnDirection->LinkEndChild(nodeRoadTD);
+
+        nodeRoadTD->SetAttribute("TurnDirection",xturn);
+
+    }
+
+    return true;
+}
+
 //--------------
 
 bool OpenDriveXmlWriter::WriteRoadBorrow(TiXmlElement *node, RoadBorrow *pRoadBorrow)

src/common/common/xodr/OpenDrive/OpenDriveXmlWriter.h

@@ -71,6 +71,7 @@ public:
 
 	//--------------
 
+    bool WriteRoadTurnDirection(TiXmlElement * node, Road * road);
     bool WriteRoadPriority(TiXmlElement * node,Road * road);
     bool WriteRoadNoavoids(TiXmlElement *node, Road* road);
     bool WriteRoadNoavoid(TiXmlElement *node,RoadNoavoid * pRoadNoavoid);

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

@@ -2318,6 +2318,32 @@ void Road::ResetPriority()
 
 //-------------------------------------------------
 
+void Road::SetRoadTurnDirection(RoadTurnDirection::TURNDIRECTION xturn)
+{
+    if(mRoadTurnDirectionVector.size() > 0)mRoadTurnDirectionVector.clear();
+    mRoadTurnDirectionVector.push_back(RoadTurnDirection(xturn));
+}
+
+//-------------------------------------------------
+
+
+int Road::GetRoadTurnDirection(RoadTurnDirection::TURNDIRECTION & xturn)
+{
+    if(mRoadTurnDirectionVector.size() == 0)return 0;
+    xturn = mRoadTurnDirectionVector[0].GetTurnDirection();
+    return 1;
+}
+
+//-------------------------------------------------
+
+
+void Road::ResetRoadTurnDirection()
+{
+    mRoadTurnDirectionVector.clear();
+}
+
+//-------------------------------------------------
+
 double Road::GetDis(const double x,const double y, const double hdg, double & fRefDis, double & fHdgDiff, int & nlane,double & s,double & refx,double & refy,double & refhdg)
 {
     unsigned int ngbsize = static_cast<unsigned int >(mGeometryBlockVector.size());

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

@@ -106,6 +106,8 @@ private:
 
     vector<RoadPriority> mRoadPriorityVector;
 
+    vector<RoadTurnDirection> mRoadTurnDirectionVector;   //  Use to define road in juction  is a left or right or straight road.
+
     vector<surface_CRG> mSurfaceCRGVector;
 
     vector<Railroad_Switch> mRailroadSwitchVector;
@@ -487,6 +489,10 @@ public:
     int GetRoadPriority(int & nPriority);
     void ResetPriority();
 
+    void SetRoadTurnDirection(RoadTurnDirection::TURNDIRECTION xturn);
+    int GetRoadTurnDirection(RoadTurnDirection::TURNDIRECTION & xturn);
+    void ResetRoadTurnDirection();
+
     double GetDis(const double x,const double y, const double hdg, double & fRefDis, double & fHdgDiff, int & nlane,double & s,double & refx,double & refy,double & refhdg); //fRefDis distance to reference line,  nlane if dis is 0, nlane which lane is distance is 0
 
     short int GetLaneCoords(double s_check, double t_check,double &retX, double &retY, double &retZ,double &retHDG);

src/common/common/xodr/OpenDrive/userData.cpp

@@ -95,3 +95,20 @@ int RoadPriority::GetPriority()
 {
     return mnPriority;
 }
+
+
+RoadTurnDirection::RoadTurnDirection(TURNDIRECTION xturn)
+{
+    mturndirection = xturn;
+}
+
+void RoadTurnDirection::SetTurnDirection(TURNDIRECTION xturn)
+{
+    mturndirection = xturn;
+}
+
+RoadTurnDirection::TURNDIRECTION RoadTurnDirection::GetTurnDirection()
+{
+    return mturndirection;
+}
+

src/common/common/xodr/OpenDrive/userData.h

@@ -55,4 +55,24 @@ public:
     int GetPriority();
 };
 
+class RoadTurnDirection
+{
+public:
+    enum TURNDIRECTION
+    {
+        LEFT = 1,
+        RIGHT = 2,
+        STRAIGHT = 3
+    };
+private:
+    TURNDIRECTION mturndirection;
+
+public:
+    RoadTurnDirection(TURNDIRECTION xturn);
+
+    void SetTurnDirection(TURNDIRECTION xturn);
+    TURNDIRECTION GetTurnDirection();
+
+};
+
 #endif // USERDATA_H

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -1473,28 +1473,30 @@ Lane * xodrfunc::GetLaneByID(LaneSection *pLS, int nlane)
     return 0;
 }
 
-void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
+int xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
 {
     int ngeobsize = static_cast<int>(pRoad->GetGeometryBlockCount());
     int i;
     bool boptimize = false;
     for(i=1;i<(ngeobsize-1);i++)
     {
+        //If have big angle change, optimize center geometry(pgeo1)
         RoadGeometry *  pgeo1 =  pRoad->GetGeometryBlock(i)->GetGeometryAt(0);
         RoadGeometry *  pgeo2 =  pRoad->GetGeometryBlock(i+1)->GetGeometryAt(0);
         RoadGeometry *  pgeo0 =  pRoad->GetGeometryBlock(i-1)->GetGeometryAt(0);
 
-        double x,y,hdgend;
-        pgeo0->GetCoords(pgeo0->GetS() + pgeo0->GetLength()- 0.001,x,y,hdgend);
-
-        std::cout<<"i: "<<i<<" front end hdg: "<<hdgend<<" now hdg:  "<<pgeo1->GetHdg()<<std::endl;
+//        double x,y,hdgend;
+//        pgeo0->GetCoords(pgeo0->GetS() + pgeo0->GetLength(),x,y,hdgend);
+ //       std::cout<<"i: "<<i<<" front end hdg: "<<hdgend<<" now hdg:  "<<pgeo1->GetHdg()<<std::endl;
 
         if((pgeo1 == NULL) || (pgeo2 == NULL) || (pgeo0 == NULL))
         {
+            std::cout<<" xodrfunc::optimizeroad, have pgeo0 pgeo2 pgeo0 have NULL geo."<<std::endl;
             continue;
         }
 
-        if((pgeo1->GetLength()<5)  && (pgeo1->GetLength() > 0.1) && ((pgeo1->GetGeomType() == 0) || (pgeo1->GetGeomType() == 4)))
+        //If a small geo, and type is line parampoly3
+        if((pgeo0->GetLength()>1.0) && (pgeo1->GetLength()<5)  && (pgeo1->GetLength() > 0.1) && ((pgeo1->GetGeomType() == 0) || (pgeo1->GetGeomType() == 4)))
         {
             double fhdgdiff = pgeo2->GetHdg() - pgeo1->GetHdg();
             while(fhdgdiff>M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
@@ -1515,10 +1517,11 @@ void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
                     {
 
                         double fobjx,fobjy;
+                        //Back Get object optimize point
                         fobjx = pgeo2->GetX() + pgeo1->GetLength() * cos(pgeo2->GetHdg() + M_PI);
                         fobjy = pgeo2->GetY() + pgeo1->GetLength() * sin(pgeo2->GetHdg() + M_PI);
 
-                        //Git dis
+                        //Git dis from old point to optimize point
                         double fdis = sqrt(pow(pgeo1->GetX() -fobjx,2) + pow(pgeo1->GetY() - fobjy,2));
                         qDebug("dis, %f",fdis);
 
@@ -1568,7 +1571,7 @@ void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
                             pvectorgeob->insert(pvectorgeob->begin()+i-1,newline);
                         }
 
-                        //CChange to new geo.
+                        //Change to new geo.
                         pgeo0 = pRoad->GetGeometryBlock(i-1)->GetGeometryAt(0);
 
                         //Change geo1
@@ -1621,6 +1624,91 @@ void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
 
                 }
 
+                if(pgeo0->GetGeomType() == 0)
+                {
+
+                    double fobjx,fobjy;
+                    //Back Get object optimize point
+                    fobjx = pgeo2->GetX() + pgeo1->GetLength() * cos(pgeo2->GetHdg() + M_PI);
+                    fobjy = pgeo2->GetY() + pgeo1->GetLength() * sin(pgeo2->GetHdg() + M_PI);
+
+                    //Git dis from old point to optimize point
+                    double fdis = sqrt(pow(pgeo1->GetX() -fobjx,2) + pow(pgeo1->GetY() - fobjy,2));
+                    qDebug("line dis, %f",fdis);
+
+                    double fopvalue = fdis;
+
+                    //If dis > maxopvalue, set dis to maxopvalue,  and change first  geo end point.
+                    if(fopvalue > maxopvalue)
+                    {
+                        fopvalue = maxopvalue;
+                        double fhdg = CalcHdg(QPointF(pgeo1->GetX(),pgeo1->GetY()),QPointF(fobjx,fobjy));
+                        fobjx = pgeo1->GetX() + fopvalue * cos(fhdg);
+                        fobjy = pgeo1->GetY() + fopvalue * sin(fhdg);
+                    }
+
+                    //Calc dis from geo0 startpoint to obj point
+                    double flinedis = sqrt(pow(fobjx - pgeo0->GetX(),2)+ pow(fobjy - pgeo0->GetY(),2));
+                    //Calc hdg from geo0 startpoint to obj point
+                    double fhdglinetoline = CalcHdg(QPointF(pgeo0->GetX(),pgeo0->GetY()),QPointF(fobjx,fobjy));
+
+                    pgeo0->SetHdg(fhdglinetoline);
+                    pgeo0->SetLength(flinedis);
+
+                    //geo1 hdg
+                    double flinehdg = CalcHdg(QPointF(fobjx,fobjy),QPointF(pgeo2->GetX(),pgeo2->GetY()));
+
+
+                    //Change geo1
+                    if(pgeo1->GetGeomType() == 0)
+                    {
+                        pgeo1->SetX(fobjx);
+                        pgeo1->SetY(fobjy);
+                        pgeo1->SetHdg(flinehdg);
+                        pgeo1->SetLength(sqrt(pow(pgeo2->GetX() - fobjx,2)+pow(pgeo2->GetY() - fobjy,2)));
+                    }
+                    else
+                    {
+                        double startx,starty,starthdg,endx,endy,endhdg;
+                        endx = pgeo2->GetX();endy = pgeo2->GetY();endhdg = pgeo2->GetEndHdg();
+                        pgeo0->GetCoords(pgeo0->GetS() + pgeo0->GetLength(),startx,starty,starthdg);
+                        std::vector<geobase> xvectorgeo = geofit::CreateBezierGeo(startx,starty,starthdg,endx,endy,endhdg);
+
+                        if(xvectorgeo.size()>0)
+                        {
+                            if(xvectorgeo[0].mnType == 0)
+                            {
+                                std::vector<GeometryBlock> * pvectorgeob =  pRoad->GetGeometryBlockVector();
+                                GeometryBlock newline;
+                                newline.AddGeometryLine(pgeo1->GetS(),xvectorgeo[0].mfX,xvectorgeo[0].mfY,xvectorgeo[0].mfHdg,xvectorgeo[0].mfLen);
+                                pvectorgeob->erase(pvectorgeob->begin()+i);
+                                pvectorgeob->insert(pvectorgeob->begin()+i,newline);
+                            }
+                            else
+                            {
+                                std::vector<GeometryBlock> * pvectorgeob =  pRoad->GetGeometryBlockVector();
+                                GeometryBlock newpp3;
+                                newpp3.AddGeometryParamPoly3(pgeo1->GetS(),xvectorgeo[0].mfX,xvectorgeo[0].mfY,xvectorgeo[0].mfHdg,xvectorgeo[0].mfLen,
+                                                             xvectorgeo[0].mfu[0],xvectorgeo[0].mfu[1],xvectorgeo[0].mfu[2],xvectorgeo[0].mfu[3],
+                                                             xvectorgeo[0].mfv[0],xvectorgeo[0].mfv[1],xvectorgeo[0].mfv[2],xvectorgeo[0].mfv[3]);
+                                pvectorgeob->erase(pvectorgeob->begin()+i);
+                                pvectorgeob->insert(pvectorgeob->begin()+i,newpp3);
+                            }
+                        }
+                        else
+                        {
+                            std::cout<<"Optimize road ,CreateBezierGeo Fail."<<std::endl;
+                        }
+                    }
+
+                    boptimize = true;
+
+
+
+
+                }
+
+
 
                 i = i+2;  //Because Optimize, so move 2 step.
             }
@@ -1642,4 +1730,7 @@ void xodrfunc::optimizeroad(Road * pRoad,const double maxopvalue)
         pRoad->SetRoadLength(snow);
     }
 
+    if(boptimize)return 1;
+    return 0;
+
 }

src/common/common/xodr/xodrfunc/xodrfunc.h

@@ -83,7 +83,7 @@ public:
     static int GetDrivingLane(Road * pRoad,const int nLS,const int nsuggestlane);
     static Lane * GetLaneByID(LaneSection * pLS,int nlane);
 
-    static void optimizeroad(Road * pRoad,const double maxopvalue = 0.5);
+    static int optimizeroad(Road * pRoad,const double maxopvalue = 0.1);
 };
 
 #endif // XODRFUNC_H

src/tool/map_collectfromveh/collectconvert.cpp

@@ -3,6 +3,8 @@
 #include "gnss_coordinate_convert.h"
 #include "geofit.h"
 
+#include "xodrfunc.h"
+
 #include "autoconnect.h"
 
 int groadid = 10001;
@@ -39,6 +41,11 @@ void CollectConvert::threadconvert()
 
 void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxodr)
 {
+
+    double LINE_ERROR = 0.10;
+    double Optimize_Value = 0.10;
+
+
     if(proad->mpoints_size() < 2)
     {
         std::cout<<" road : "<<proad->strroadname()<<" only "
@@ -79,9 +86,6 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
     }
 
 
-    double LINE_ERROR = 0.20;
-
-
     std::vector<geobase> xvectorgeo;
 
     bool bComplete = false;
@@ -434,6 +438,8 @@ void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxod
     }
 
 
+    xodrfunc::optimizeroad(p,Optimize_Value);
+
 
 
 }

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -6175,7 +6175,21 @@ void MainWindow::on_actionOptimize_Road_triggered()
 
     if(pRoad == NULL)return;
 
-    xodrfunc::optimizeroad(pRoad);
+    int nrtn = xodrfunc::optimizeroad(pRoad);
+
+    if(nrtn == 1)
+    {
+        mbRefresh = true;
+        update();
+        if(mpCBViewMode->currentIndex() == 1)
+        {
+            UpdateScene();
+        }
+    }
+    else
+    {
+        std::cout<<" no optimize, so not need update."<<std::endl;
+    }
 
 }