add simple_planning_simulator. not complete.

src/tool/simple_planning_simulator/.gitignore

@@ -0,0 +1,74 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+CMakeLists.txt.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/tool/simple_planning_simulator/main.cpp

@@ -0,0 +1,11 @@
+#include "mainwindow.h"
+
+#include <QApplication>
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    MainWindow w;
+    w.show();
+    return a.exec();
+}

src/tool/simple_planning_simulator/mainwindow.cpp

@@ -0,0 +1,273 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+#include "roaddigit.h"
+#include "xodrscenfunc.h"
+
+
+#include <QFileDialog>
+#include <QMessageBox>
+
+#define VIEW_WIDTH 1000
+#define VIEW_HEIGHT 1000
+
+
+double glon0 = 117.0866293;
+double glat0 = 39.1364713;
+
+MainWindow::MainWindow(QWidget *parent)
+    : QMainWindow(parent)
+    , ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+
+    myview = new MyView(ui->centralwidget);
+    myview->setObjectName(QStringLiteral("graphicsView"));
+    myview->setGeometry(QRect(30, 30, 600, 600));
+
+    connect(myview,SIGNAL(dbclickxy(double,double)),this,SLOT(onClickXY(double,double)));
+
+    image = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);//画布的初始化大小设为300*300，使用32位颜色
+    myview->setCacheMode(myview->CacheBackground);
+
+    painter = new QPainter(image);
+
+    painter->end();
+
+    scene = new QGraphicsScene;
+
+    //    mpscene = new  QGraphicsScene;//(-300, -300, 600, 600);
+    mpscene = new  QGraphicsScene(0,0,VIEW_WIDTH,VIEW_HEIGHT);
+    mpscene->setBackgroundBrush(Qt::darkGreen);
+
+    mnFontHeight =  ui->centralwidget->fontMetrics().height();
+
+    QTabWidget * p = new QTabWidget(ui->centralwidget);
+    p->setGeometry(30,30,300,300);
+
+  //  CreateTab1View(p);
+
+    mpLabel_Status = new QLabel(this);
+    ui->statusbar->addPermanentWidget(mpLabel_Status);
+
+    mTabMain = p;
+
+    CreateCar();
+
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}
+
+void MainWindow::resizeEvent(QResizeEvent *event)
+{
+    (void)event;
+    QSize sizemain = ui->centralwidget->size();
+    myview->setGeometry(0,30,sizemain.width()-mnFontHeight * 22 - 30,sizemain.height());
+    mTabMain->setGeometry(sizemain.width()-mnFontHeight * 22,30,mnFontHeight * 22,sizemain.height()-50);
+
+
+}
+
+void MainWindow::paintEvent(QPaintEvent *)
+{
+    myview->setScene(mpscene);
+    myview->show();
+}
+
+void MainWindow::onClickXY(double x,double y)
+{
+
+}
+
+void MainWindow::on_actionLoad_triggered()
+{
+    QString str = QFileDialog::getOpenFileName(this,"Load XODR",".","*.xodr");
+    if(str.isEmpty())return;
+
+    OpenDrive * pxodr = new OpenDrive();  //because add to xodr,so don't delete
+    OpenDriveXmlParser x(pxodr);
+    if(!x.ReadFile(str.toStdString()))
+    {
+        QMessageBox::warning(this,"warn","Can't  load xodr file.");
+        return;
+    }
+
+
+    unsigned short int revMajor,revMinor;
+    std::string name,date;
+    float version;
+    double north,south,east,west,lat0,lon0,hdg0;
+    if(pxodr->GetHeader() != 0)
+    {
+        pxodr->GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+    }
+    else
+    {
+        lat0 = 39.0;
+        lon0 = 119.0;
+    }
+
+    glat0 = lat0;
+    glon0 = lon0;
+
+    mxodr = *pxodr;
+
+    UpdateScene();
+}
+
+
+#include <QtGui>
+
+void MainWindow::UpdateScene()
+{
+
+    int i;
+    int nsize = mvectorviewitem.size();
+    for(i=0;i<nsize;i++)
+    {
+        mpscene->removeItem(mvectorviewitem.at(i));
+        delete mvectorviewitem.at(i);
+    }
+    nsize = mvectorgeoitem.size();
+    for(i=0;i<nsize;i++)
+    {
+        mpscene->removeItem(mvectorgeoitem.at(i));
+        delete mvectorgeoitem.at(i);
+    }
+    mvectorviewitem.clear();
+    mvectorgeoitem.clear();
+
+    nsize = mxodr.GetRoadCount();
+
+    std::vector<RoadDigit> xvectorrd;
+    for(i=0;i<nsize;i++)
+    {
+        RoadDigit xrd(mxodr.GetRoad(i),5.0);
+        xvectorrd.push_back(xrd);
+        //       UpdateSceneRoad(mxodr.GetRoad(i));
+        //        qDebug("update road %d",i);
+    }
+    for(i=0;i<nsize;i++)
+    {
+
+        std::vector<QGraphicsPathItem *> xvectorlanepath = xodrscenfunc::GetRoadLaneItem(&(xvectorrd[i]));
+        int j;
+        int ncount = xvectorlanepath.size();
+        for(j=0;j<ncount;j++)
+        {
+            QGraphicsPathItem * pitem = xvectorlanepath[j];
+            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+            mpscene->addItem(pitem);
+            mvectorviewitem.push_back(pitem);
+        }
+
+    }
+
+    for(i=0;i<nsize;i++)
+    {
+
+        std::vector<QGraphicsPathItem *> xvectormarkpath = xodrscenfunc::GetRoadMarkItem(&(xvectorrd[i]));
+        int j;
+        int ncount = xvectormarkpath.size();
+        for(j=0;j<ncount;j++)
+        {
+            QGraphicsPathItem * pitem = xvectormarkpath[j];
+            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+            mpscene->addItem(pitem);
+            mvectorviewitem.push_back(pitem);
+        }
+    }
+
+    for(i=0;i<nsize;i++)
+    {
+
+        std::vector<QGraphicsTextItem *> xvectortext;
+        std::vector<QGraphicsPathItem *> xvectorparkmark;
+        std::vector<QGraphicsPathItem *> xvectorparkpath = xodrscenfunc::GetRoadParkingItem(&(xvectorrd[i]),xvectortext,
+                                                                                            xvectorparkmark);
+        int j;
+        int ncount = xvectorparkpath.size();
+        for(j=0;j<ncount;j++)
+        {
+            QGraphicsPathItem * pitem = xvectorparkpath[j];
+            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+            mpscene->addItem(pitem);
+            mvectorviewitem.push_back(pitem);
+        }
+        ncount = xvectortext.size();
+        for(j=0;j<ncount;j++)
+        {
+            QGraphicsTextItem * pitem = xvectortext[j];
+            //           pitem->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + (-24.0),-mfViewMoveY+VIEW_HEIGHT/2.0+(-102.0));
+            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + pitem->pos().x(),-mfViewMoveY+VIEW_HEIGHT/2.0+pitem->pos().y());
+            mpscene->addItem(pitem);
+        }
+        ncount = xvectorparkmark.size();
+        for(j=0;j<ncount;j++)
+        {
+            QGraphicsPathItem * pitem = xvectorparkmark[j];
+            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+            mpscene->addItem(pitem);
+            mvectorviewitem.push_back(pitem);
+        }
+    }
+
+
+    SetCarPos(30,10,M_PI/2.0);
+
+
+}
+
+void MainWindow::CreateCar()
+{
+    QGraphicsPixmapItem * ppix;
+    ppix = new QGraphicsPixmapItem();
+    ppix->setPixmap(QPixmap(":/car.png"));
+    QRectF xr = ppix->boundingRect();
+    double fvehwidth = 2.0;
+    double fvehlen = fvehwidth*(xr.width()/xr.height());
+    double fhdg = 0;
+    double fhead = fhdg * 180.0/M_PI *(-1.0);
+    double fscale =  2.0/xr.height();
+
+    mfvehwidth = fvehwidth;
+    mfvehlen = fvehlen;
+
+    double x0,y0;
+    x0 = fvehlen/2.0;
+    y0 = fvehwidth/2.0 *(-1.0);
+    double x1,y1;
+    double hdgrt = fhdg;//法 fhead *M_PI/180.0;
+    x1 = x0 * cos(hdgrt) - y0 * sin(hdgrt);
+    y1 = x0 * sin(hdgrt) + y0 * cos(hdgrt);
+
+    ppix->setRotation(fhead);
+    ppix->setScale(fscale);
+    ppix->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + 0 - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 + y1);
+    mpscene->addItem(ppix);
+    ppix->setZValue(100.0);
+    mppixcar = ppix;
+}
+
+void MainWindow::SetCarPos(double x,double y, double fhdg)
+{
+    double fhead = fhdg * 180.0/M_PI *(-1.0);
+
+    double x0,y0;
+    x0 = mfvehlen/2.0;
+    y0 = mfvehwidth/2.0 *(-1.0);
+    double x1,y1;
+    double hdgrt = fhdg;//法 fhead *M_PI/180.0;
+    x1 = x0 * cos(hdgrt) - y0 * sin(hdgrt);
+    y1 = x0 * sin(hdgrt) + y0 * cos(hdgrt);
+
+    mppixcar->setRotation(fhead);
+
+
+    mppixcar->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + x - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - y + y1);
+
+}
+

src/tool/simple_planning_simulator/mainwindow.h

@@ -0,0 +1,75 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+
+#include <QLabel>
+
+#include "OpenDrive/OpenDrive.h"
+#include "OpenDrive/OpenDriveXmlParser.h"
+
+#include "myview.h"
+
+QT_BEGIN_NAMESPACE
+namespace Ui { class MainWindow; }
+QT_END_NAMESPACE
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    MainWindow(QWidget *parent = nullptr);
+    ~MainWindow();
+
+private slots:
+    virtual void paintEvent(QPaintEvent *);
+
+    void onClickXY(double x,double y);
+
+    void on_actionLoad_triggered();
+
+private:
+    Ui::MainWindow *ui;
+
+    QImage *image;
+    QPainter *painter;
+    MyView *myview;
+    QTimer *timer;
+    QGraphicsScene *scene;
+
+    QGraphicsScene * mpscene;
+
+    QGraphicsPixmapItem * mppixcar;
+
+    QLabel * mpLabel_Status;
+
+    QTabWidget * mTabMain;
+    void CreateTab1View(QTabWidget * p);
+
+    double mfViewMoveX = 0;
+    double mfViewMoveY = 0;
+
+    double mfvehwidth;
+    double mfvehlen;
+
+private:
+    int mnFontHeight;
+
+    OpenDrive mxodr;
+
+    void UpdateScene();
+
+    std::vector<QGraphicsPathItem *> mvectorviewitem;
+    std::vector<QGraphicsPathItem *> mvectorgeoitem;
+
+
+public:
+    void resizeEvent(QResizeEvent *event);
+
+private:
+    void CreateCar();
+    void SetCarPos(double x,double y, double fhdg);
+
+};
+#endif // MAINWINDOW_H

src/tool/simple_planning_simulator/mainwindow.ui

@@ -0,0 +1,49 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>800</width>
+    <height>600</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <property name="windowIcon">
+   <iconset resource="sim.qrc">
+    <normaloff>:/car.png</normaloff>:/car.png</iconset>
+  </property>
+  <widget class="QWidget" name="centralwidget"/>
+  <widget class="QMenuBar" name="menubar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>800</width>
+     <height>27</height>
+    </rect>
+   </property>
+   <widget class="QMenu" name="menuFile">
+    <property name="title">
+     <string>File</string>
+    </property>
+    <addaction name="actionLoad"/>
+   </widget>
+   <addaction name="menuFile"/>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+  <action name="actionLoad">
+   <property name="text">
+    <string>Load</string>
+   </property>
+  </action>
+ </widget>
+ <resources>
+  <include location="sim.qrc"/>
+ </resources>
+ <connections/>
+</ui>

src/tool/simple_planning_simulator/sim.qrc

@@ -0,0 +1,5 @@
+<RCC>
+    <qresource prefix="/">
+        <file>car.png</file>
+    </qresource>
+</RCC>

src/tool/simple_planning_simulator/simple_planning_simulator.pro

@@ -0,0 +1,81 @@
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+CONFIG += c++17
+
+QMAKE_LFLAGS += -no-pie
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    ../../common/common/xodr/xodrfunc/roadsample.cpp \
+    ../map_lanetoxodr/TinyXML/tinystr.cpp \
+    ../map_lanetoxodr/TinyXML/tinyxml.cpp \
+    ../map_lanetoxodr/TinyXML/tinyxmlerror.cpp \
+    ../map_lanetoxodr/TinyXML/tinyxmlparser.cpp \
+    ../map_lanetoxodr/const.cpp \
+    ../map_lanetoxodr/fresnl.cpp \
+    ../map_lanetoxodr/function/circlefitting.cpp \
+    ../map_lanetoxodr/function/geofit.cpp \
+    ../map_lanetoxodr/polevl.c \
+    ../map_lanetoxodr/view/myview.cpp \
+    ../map_lanetoxodr/view/roaddigit.cpp \
+    ../map_lanetoxodr/view/roadviewitem.cpp \
+    ../map_lanetoxodr/view/viewcreate.cpp \
+    ../map_lanetoxodr/view/xodrscenfunc.cpp \
+    main.cpp \
+    mainwindow.cpp
+
+HEADERS += \
+    ../../common/common/xodr/xodrfunc/roadsample.h \
+    ../map_lanetoxodr/TinyXML/tinystr.h \
+    ../map_lanetoxodr/TinyXML/tinyxml.h \
+    ../map_lanetoxodr/function/circlefitting.h \
+    ../map_lanetoxodr/function/geofit.h \
+    ../map_lanetoxodr/view/myview.h \
+    ../map_lanetoxodr/view/roaddigit.h \
+    ../map_lanetoxodr/view/roadviewitem.h \
+    ../map_lanetoxodr/view/viewcreate.h \
+    ../map_lanetoxodr/view/xodrscenfunc.h \
+    mainwindow.h
+
+FORMS += \
+    mainwindow.ui
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+
+INCLUDEPATH += $$PWD/../map_lanetoxodr
+INCLUDEPATH += $$PWD/../map_lanetoxodr/view
+INCLUDEPATH += $$PWD/../map_lanetoxodr/function
+INCLUDEPATH += $$PWD/../map_lanetoxodr/TinyXML
+
+!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
+    error( "Couldn't find the OpenDrive.pri file!" )
+}
+
+!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
+    error( "Couldn't find the xodrfunc.pri file!" )
+}
+
+unix:INCLUDEPATH += /usr/include/eigen3
+win32:INCLUDEPATH += D:\File\soft\eigen
+
+DEFINES += NOTINPILOT
+
+INCLUDEPATH += $$PWD/../../include/msgtype
+
+INCLUDEPATH += $$PWD/../../common/common/xodr
+
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
+
+unix:LIBS += -lboost_thread -lboost_system -lboost_serialization -lprotobuf
+
+RESOURCES += \
+    sim.qrc

src/tool/tool_calcgps/mainwindow.cpp

@@ -79,7 +79,7 @@ void MainWindow::on_pushButton_LonLatConvertRel_clicked()
     normalhdg(hdg_rel);
 
     x_rel = x_rel_raw * cos(-hdg_base) - y_rel_raw * sin(-hdg_base);
-    y_rel = x_rel_raw * sin(-hdg_base) - y_rel_raw * cos(-hdg_base);
+    y_rel = x_rel_raw * sin(-hdg_base) + y_rel_raw * cos(-hdg_base);
 
     char strout[1000];
     char strtem[100];
@@ -137,7 +137,7 @@ void MainWindow::on_pushButton_RelConvertLonLat_clicked()
     normalhdg(hdg_base);
 
     x_rel_raw = x_rel * cos(hdg_base) - y_rel * sin(hdg_base);
-    y_rel_raw = x_rel * sin(hdg_base) - y_rel * cos(hdg_base);
+    y_rel_raw = x_rel * sin(hdg_base) + y_rel * cos(hdg_base);
 
     hdg_test = hdg_base + hdg_rel;