change adcxodrviewer2.

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

@@ -2532,8 +2532,8 @@ short int Road::GetLaneCoords(double s_check, double t_check,double &retX, doubl
 
     retHDG = fRefHDG;
 
-    retX = fRefX + t_check*cos(fSuperElevationValue)*cos(retHDG + M_PI/2.0) + fOffValue;
-    retY = fRefY + t_check*cos(fSuperElevationValue)*sin(retHDG + M_PI/2.0) + fOffValue;
+    retX = fRefX + t_check*cos(fSuperElevationValue)*cos(retHDG + M_PI/2.0) + fOffValue*cos(retHDG+M_PI/2.0);
+    retY = fRefY + t_check*cos(fSuperElevationValue)*sin(retHDG + M_PI/2.0) + fOffValue*sin(retHDG+M_PI/2.0);
 
     double fElevation = GetElevationValue(s_check);
 

src/common/common/xodr/OpenDrive/RoadGeometry.cpp

@@ -664,7 +664,7 @@ void GeometryPoly3::UpdateSamplePoint()
             gsp.y = xvectorgeosample[ipos1].y;
             if(ipos1 == 0)
             {
-                gsp.fHdg = mHdg;
+                gsp.fHdg = 0;//mHdg;
             }
             else
             {
@@ -689,11 +689,11 @@ void GeometryPoly3::UpdateSamplePoint()
             gsp.y = y1 + fratio * (y2 - y1);
             if(mvectorgeosample.size() == 0)
             {
-                gsp.fHdg = mHdg;
+                gsp.fHdg = 0;//mHdg;
             }
             else
             {
-                gsp.fHdg = (mvectorgeosample[mvectorgeosample.size() -1].x,mvectorgeosample[mvectorgeosample.size() -1].y,
+                gsp.fHdg = CalcHdg(mvectorgeosample[mvectorgeosample.size() -1].x,mvectorgeosample[mvectorgeosample.size() -1].y,
                         gsp.x,gsp.y);
             }
         }
@@ -702,7 +702,8 @@ void GeometryPoly3::UpdateSamplePoint()
         s = s+ ds;
     }
 
-//    vector<geosamplepoint> * pxvectorgeosample = &mvectorgeosample;
+
+    vector<geosamplepoint> * pxvectorgeosample = &mvectorgeosample;
     mbHaveSample = true;
 
 }
@@ -762,9 +763,9 @@ void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double
         double temX,temY,temHDG;
         if(ipos<=0)
         {
-            temX = mvectorgeosample[ipos].x;
-            temY = mvectorgeosample[ipos].y;
-            temHDG = mHdg;
+            temX = mvectorgeosample[0].x;
+            temY = mvectorgeosample[0].y;
+            temHDG = mvectorgeosample[0].fHdg;;
         }
         else
         {

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

@@ -79,9 +79,19 @@ int RoadSample::SampleRoad(Road * pRoad)
         }
         nSec++;
 
+        fS = sstart_Section;
         while(fS<send_Section)
         {
-
+            bool bInserPoint = false;
+            if(fabs(send_Section - fS)<=fStep)
+            {
+                fS = send_Section;  //If Section End,Same Point
+                if(i !=(nLSCount -1))
+                {
+                    fS = send_Section - 0.0000001; //if not the last,set a value small to sample;
+                }
+                bInserPoint = true;
+            }
             double fRefX,fRefY,fRefZ,fRefHdg,fOffsetValue;
             pRoad->GetGeometryCoords(fS,fRefX,fRefY,fRefHdg);
             fRefZ = pRoad->GetElevationValue(fS);
@@ -89,18 +99,9 @@ int RoadSample::SampleRoad(Road * pRoad)
 
             if(fabs(fOffsetValue) > mfMaxLaneOff)mfMaxLaneOff = fabs(fOffsetValue);
 
-            if(fabs(send_Section - fS)<0.1)
-            {
-                fS = send_Section;  //If Section End,Same Point
-//                if(i == (nLSCount - 1))
-//                {
-//                    fS = send_Section;
-//                }
-//                else
-//                {
-//                    fS = send_Section;
-//                }
-            }
+
+
+
 
             bool bIsCrossFeature = false;
 
@@ -141,7 +142,7 @@ int RoadSample::SampleRoad(Road * pRoad)
  //           SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
 
 
-            bool bInserPoint = false;
+
 
             if(bHaveLast == false)
             {
@@ -333,7 +334,7 @@ int RoadSample::SampleRoad(Road * pRoad)
         }
     }
 
- //   std::vector<RoadPoint> * pvectorRoadPoint = &mvectorRoadPoint;
+    std::vector<RoadPoint> * pvectorRoadPoint = &mvectorRoadPoint;
 
     std::cout<<"Road: "<<mstrroadid<<" max "<<mfRefX_max<<" "<<mfRefY_max<<"  min "<<mfRefX_min<<" "<<mfRefY_min<<std::endl;
 
@@ -609,7 +610,7 @@ double RoadSample::GetYMin()
     return  mfY_min;
 }
 
-bool RoadSample::PointInRoad(const double x, const double y,  double &s,double & t,int & nlane)
+bool RoadSample::PointInRoad(const double x, const double y,  double &s,double & t,int & nlane, std::string & strlanetype)
 {
     if((x<mfX_min)||(x>mfX_max))
     {
@@ -658,11 +659,14 @@ bool RoadSample::PointInRoad(const double x, const double y,  double &s,double &
                     if(p1.mvectorLanePoint[j].nLane<=0)
                     {
                         nlane = p1.mvectorLanePoint[j+1].nLane;
+                        strlanetype =  p1.mvectorLanePoint[j+1].mstrLaneType;
                     }
                     else
                     {
                         nlane = p1.mvectorLanePoint[j].nLane;
+                        strlanetype =  p1.mvectorLanePoint[j].mstrLaneType;
                     }
+
                     return true;
                 }
             }

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

@@ -94,7 +94,7 @@ public:
     double GetYMax();
     double GetYMin();
 
-    bool PointInRoad(const double x, const double y,  double & s, double & t, int & nlane);
+    bool PointInRoad(const double x, const double y,  double & s, double & t, int & nlane, std::string & strlanetype);
 
 private:
     std::string mstrroadid;

src/tool/map_lanetoxodr/view/myview.cpp

@@ -64,8 +64,8 @@ void MyView::zoomIn()
 //    qDebug("%d %d ",width,hgt);
 
 
-    int centery = (psV->value() + psV->size().height()/2)/beishu;
-    int centerx = (psH->value() + psH->size().width()/2)/beishu;
+    double centery = (psV->value() + psV->size().height()/2)/beishu;
+    double centerx = (psH->value() + psH->size().width()/2)/beishu;
 
 #ifndef ANDROID
     scale(1.1, 1.1);
@@ -105,8 +105,8 @@ void MyView::zoomOut()
     QScrollBar * psV = verticalScrollBar();
     QScrollBar * psH = horizontalScrollBar();
 
-    int centery = (psV->value() + psV->size().height()/2)/beishu;
-    int centerx = (psH->value() + psH->size().width()/2)/beishu;
+    double centery = (psV->value() + psV->size().height()/2)/beishu;
+    double centerx = (psH->value() + psH->size().width()/2)/beishu;
 
 #ifndef ANDROID
     scale(1 / 1.1, 1 / 1.1);
@@ -139,14 +139,17 @@ void MyView::mouseDoubleClickEvent(QMouseEvent *event)
     QScrollBar * psV = verticalScrollBar();
     QScrollBar * psH = horizontalScrollBar();
 
-    int centery = (psV->value() + psV->size().height()/2)/beishu;
-    int centerx = (psH->value() + psH->size().width()/2)/beishu;
+    double centery = (psV->value() + psV->size().height()/2)/beishu;
+    double centerx = (psH->value() + psH->size().width()/2)/beishu;
 
 
 //    qDebug("x is %d y is %d view center x is %d  centerx is %d",event->pos().x(),
 //           event->pos().y(),
 //           viewport()->rect().center().x(),centerx);
 
+ //   std::cout<<" event x: "<<event->pos().x()<<" event y: "<<event->pos().y()<<std::endl;
+//    std::cout<<" centerx: "<<centerx<<" event x: "<<event->pos().x()<<" viewpoint x: "<<viewport()->rect().center().x()
+//            <<" psv size: "<<psH->size().width()<<" beishu: "<<beishu<<std::endl;
     double viewx,viewy;
     if(beishu == 0)return;
     viewx = centerx +(event->pos().x() - viewport()->rect().center().x())/beishu;

src/tool/map_lanetoxodr/view/xodrscenfunc.cpp

@@ -254,6 +254,21 @@ std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample *
                                 {
                                     brushcolor = QColor(0xB2,0xB2,0xD6);
                                 }
+                                else
+                                {
+                                    if(strlanetype == "stop")
+                                    {
+                                        brushcolor = Qt::black;
+                                    }
+                                    else
+                                    {
+                                        if(strlanetype == "median")
+                                        {
+                                            brushcolor = Qt::darkGreen;
+                                        }
+
+                                    }
+                                }
                             }
                         }
                     }

src/tool/map_lanetoxodr/xvmainwindow2.cpp

@@ -112,6 +112,8 @@ void XVMainWindow::LoadXODR(QString strpath)
         return;
     }
 
+    mvectorRoadSample.clear();
+
     int nroadnum = pxodr->GetRoadCount();
     int i;
     double froadlen = 0;
@@ -280,7 +282,7 @@ void XVMainWindow::onClickXY(double x, double y)
     mfselx = selx *1.0/mfbeishu ;
     mfsely = sely *1.0/mfbeishu;
 
-    qDebug("selx is %f sely is %f ",mfselx,mfsely);
+
 //    selx = x;
 //    sely = y;
 //    mfselx = selx;
@@ -295,24 +297,46 @@ void XVMainWindow::onClickXY(double x, double y)
     rel_x = selx - mfViewMoveX + VIEW_WIDTH/2 ;
     rel_y = sely - mfViewMoveY - VIEW_HEIGHT/2;
 
+    qDebug("selx is %f sely is %f ",rel_x,rel_y);
+
     unsigned int nRoadCount = static_cast<unsigned int>(mvectorRoadSample.size());
     unsigned int i;
 
 
     int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
+    bool bFind = false;
+    char strout[1000];
     for(i=0;i<nRoadCount;i++)
     {
         double s,t;
         int nlane;
-        if(mvectorRoadSample[i].PointInRoad(rel_x,rel_y,s,t,nlane))
+        std::string strlanetype;
+        if(mvectorRoadSample[i].PointInRoad(rel_x,rel_y,s,t,nlane,strlanetype))
         {
             std::cout<<" In Road"<<mvectorRoadSample[i].GetRoadID()<<" s:"<<s<<" t:"<<t<<" nlane:"<<nlane<<std::endl;
-            char strout[1000];
-            snprintf(strout,1000,"Road:%s s:%f t:%f nlane:%d",mvectorRoadSample[i].GetRoadID().data(),s,t,nlane);
+
+            if(bFind == false)
+            {
+                snprintf(strout,1000,"x:%7.3f y:%7.3f |Road:%s s:%f t:%f nlane:%d %s",
+                         rel_x,rel_y,mvectorRoadSample[i].GetRoadID().data(),s,t,nlane,strlanetype.data());
      //       mpLabel_Status->setText(strout);
-            ui->statusbar->showMessage(strout,10000);
+                bFind = true;
+            }
+            else
+            {
+                char strtem[1000];
+                snprintf(strtem,1000,"| %s %d",mvectorRoadSample[i].GetRoadID().data(),nlane);
+                strncat(strout,strtem,1000);
+            }
         }
     }
+
+    if(bFind == false)
+    {
+        snprintf(strout,1000,"x:%7.3f y:%7.3f",rel_x,rel_y);
+    }
+
+    ui->statusbar->showMessage(strout,10000);
     int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
     std::cout<<" find use time: "<<(time2 - time1)<<std::endl;