change simple_simulator. not complete.

src/tool/simple_planning_simulator/dialogcustomobj.cpp

@@ -0,0 +1,39 @@
+#include "dialogcustomobj.h"
+#include "ui_dialogcustomobj.h"
+
+DialogCustomObj::DialogCustomObj(QWidget *parent) :
+    QDialog(parent),
+    ui(new Ui::DialogCustomObj)
+{
+    ui->setupUi(this);
+
+    ui->lineEdit_z->setText("0.0");
+    ui->lineEdit_length->setText("4.9");
+    ui->lineEdit_width->setText("1.9");
+    ui->lineEdit_height->setText("1.6");
+
+}
+
+DialogCustomObj::~DialogCustomObj()
+{
+    delete ui;
+}
+
+void DialogCustomObj::on_pushButton_Add_clicked()
+{
+    mfz = ui->lineEdit_z->text().toDouble();
+    mflength = ui->lineEdit_length->text().toDouble();
+    mfwidth = ui->lineEdit_width->text().toDouble();
+    mfheight = ui->lineEdit_height->text().toDouble();
+    accept();
+}
+
+int DialogCustomObj::GetValue(double & z, double & flength,double & fwidth, double & fheight)
+{
+    z = mfz;
+    flength = mflength;
+    fwidth = mfwidth;
+    fheight = mfheight;
+    return 0;
+}
+

src/tool/simple_planning_simulator/dialogcustomobj.h

@@ -0,0 +1,31 @@
+#ifndef DIALOGCUSTOMOBJ_H
+#define DIALOGCUSTOMOBJ_H
+
+#include <QDialog>
+
+namespace Ui {
+class DialogCustomObj;
+}
+
+class DialogCustomObj : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit DialogCustomObj(QWidget *parent = nullptr);
+    ~DialogCustomObj();
+
+private slots:
+    void on_pushButton_Add_clicked();
+
+private:
+    Ui::DialogCustomObj *ui;
+
+    double mfz,mflength,mfwidth,mfheight;
+
+public:
+    int GetValue(double & z, double & flength,double & fwidth, double & fheight);
+
+};
+
+#endif // DIALOGCUSTOMOBJ_H

src/tool/simple_planning_simulator/dialogcustomobj.ui

@@ -0,0 +1,124 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DialogCustomObj</class>
+ <widget class="QDialog" name="DialogCustomObj">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>458</width>
+    <height>338</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Dialog</string>
+  </property>
+  <widget class="QLabel" name="label">
+   <property name="geometry">
+    <rect>
+     <x>80</x>
+     <y>21</y>
+     <width>91</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>z:</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_z">
+   <property name="geometry">
+    <rect>
+     <x>213</x>
+     <y>21</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_length">
+   <property name="geometry">
+    <rect>
+     <x>210</x>
+     <y>80</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_3">
+   <property name="geometry">
+    <rect>
+     <x>77</x>
+     <y>140</y>
+     <width>91</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>width:</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_4">
+   <property name="geometry">
+    <rect>
+     <x>77</x>
+     <y>80</y>
+     <width>91</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>length:</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_width">
+   <property name="geometry">
+    <rect>
+     <x>210</x>
+     <y>140</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_5">
+   <property name="geometry">
+    <rect>
+     <x>77</x>
+     <y>200</y>
+     <width>91</width>
+     <height>41</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>height:</string>
+   </property>
+  </widget>
+  <widget class="QLineEdit" name="lineEdit_height">
+   <property name="geometry">
+    <rect>
+     <x>210</x>
+     <y>200</y>
+     <width>161</width>
+     <height>41</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="pushButton_Add">
+   <property name="geometry">
+    <rect>
+     <x>160</x>
+     <y>260</y>
+     <width>131</width>
+     <height>51</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Add</string>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

src/tool/simple_planning_simulator/mainwindow.cpp

@@ -8,6 +8,8 @@
 #include <QFileDialog>
 #include <QMessageBox>
 
+#include "dialogcustomobj.h"
+
 #define VIEW_WIDTH 1000
 #define VIEW_HEIGHT 1000
 
@@ -21,6 +23,9 @@ MainWindow::MainWindow(QWidget *parent)
 {
     ui->setupUi(this);
 
+    mfMoveX = VIEW_WIDTH/2;
+    mfMoveY = VIEW_HEIGHT/2;
+
     myview = new MyView(ui->centralwidget);
     myview->setObjectName(QStringLiteral("graphicsView"));
     myview->setGeometry(QRect(30, 30, 600, 600));
@@ -45,7 +50,7 @@ MainWindow::MainWindow(QWidget *parent)
     QTabWidget * p = new QTabWidget(ui->centralwidget);
     p->setGeometry(30,30,300,300);
 
-  //  CreateTab1View(p);
+    CreateTab1View(p);
 
     mpLabel_Status = new QLabel(this);
     ui->statusbar->addPermanentWidget(mpLabel_Status);
@@ -54,6 +59,10 @@ MainWindow::MainWindow(QWidget *parent)
 
     CreateCar();
 
+    mpTimerShowSelObj = new QTimer(this);
+    connect(mpTimerShowSelObj,SIGNAL(timeout()),this,SLOT(onTimerShowSelObj()));
+    mpTimerShowSelObj->start(100);
+
 }
 
 MainWindow::~MainWindow()
@@ -69,6 +78,143 @@ void MainWindow::resizeEvent(QResizeEvent *event)
     mTabMain->setGeometry(sizemain.width()-mnFontHeight * 22,30,mnFontHeight * 22,sizemain.height()-50);
 
 
+}
+
+void MainWindow::CreateTab1View(QTabWidget * p)
+{
+    (void)p;
+
+    QGroupBox * pGroup = new QGroupBox();
+    pGroup->setGeometry(0,0,mnFontHeight * 21,mnFontHeight * 160);
+
+    QLabel * pLabel;
+    QLineEdit * pLE;
+    QPushButton * pPB;
+    QSlider * pSlider;
+    QComboBox * pCB;
+
+
+
+    int nXPos = 10;
+    int nYPos = 30;
+
+    int i;
+
+    int nSpace = mnFontHeight * 65/10;
+    int nLEWidth = mnFontHeight * 6;
+    int nLEHeight = mnFontHeight * 3/2;
+
+    pLE = new QLineEdit(pGroup);
+    pLE->setText("");
+    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
+    mpLE_SelX = pLE;
+    nXPos = nXPos + nSpace;
+
+    pLE = new QLineEdit(pGroup);
+    pLE->setText("");
+    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
+    mpLE_SelY = pLE;
+    nXPos = nXPos + nSpace;
+
+
+
+    nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    pLE = new QLineEdit(pGroup);
+    pLE->setText("");
+    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
+    mpLE_SelLon = pLE;
+    nXPos = nXPos + nSpace;
+
+    pLE = new QLineEdit(pGroup);
+    pLE->setText("");
+    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
+    mpLE_SelLat = pLE;
+    nXPos = nXPos + nSpace;
+
+    pLE = new QLineEdit(pGroup);
+    pLE->setText("");
+    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
+    pLE->setText("360");
+    mpLE_SelHeading = pLE;
+
+ //   nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    pPB = new QPushButton(pGroup);
+    pPB->setText(tr("设置主车位置"));
+    pPB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight*3/2);
+    mpPBSetCarPos = pPB;
+
+    connect(mpPBSetCarPos,SIGNAL(clicked(bool)),this,SLOT(onClickSetCarPos()));
+
+ //   nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    pPB = new QPushButton(pGroup);
+    pPB->setText(tr("设置行人障碍物"));
+    pPB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight*3/2);
+    mpPBAddPedObj = pPB;
+
+    connect(mpPBAddPedObj,SIGNAL(clicked(bool)),this,SLOT(onClickAddPedObj()));
+
+    nYPos = nYPos + mnFontHeight * 2;
+
+    nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    pPB = new QPushButton(pGroup);
+    pPB->setText(tr("设置车辆障碍物"));
+    pPB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight*3/2);
+    mpPBAddCarObj = pPB;
+
+    connect(mpPBAddCarObj,SIGNAL(clicked(bool)),this,SLOT(onClickAddCarObj()));
+
+    nYPos = nYPos + mnFontHeight * 2;
+
+    nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    pPB = new QPushButton(pGroup);
+    pPB->setText(tr("设置自定义障碍物"));
+    pPB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight*3/2);
+    mpPBAddCustomObj = pPB;
+
+    connect(mpPBAddCustomObj,SIGNAL(clicked(bool)),this,SLOT(onClickAddCustomObj()));
+
+    nYPos = nYPos + mnFontHeight * 2;
+
+    nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    pCB = new QComboBox(pGroup);
+    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight*3/2);
+    mpCBObj = pCB;
+    connect(mpCBObj,SIGNAL(currentIndexChanged(int)),this,SLOT(onCurrentIndexChanged(int)));
+
+    nXPos = nXPos + nSpace;
+
+    pPB = new QPushButton(pGroup);
+    pPB->setText(tr("删除障碍物"));
+    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight*3/2);
+    mpPBDelObj = pPB;
+
+    connect(mpPBDelObj,SIGNAL(clicked(bool)),this,SLOT(onClickDelObj()));
+
+    QScrollArea * pScroll = new QScrollArea();
+    pScroll->setWidget(pGroup);
+
+    p->addTab(pScroll,"Simulate");
+
+
+
 }
 
 void MainWindow::paintEvent(QPaintEvent *)
@@ -79,7 +225,30 @@ void MainWindow::paintEvent(QPaintEvent *)
 
 void MainWindow::onClickXY(double x,double y)
 {
+    (void)x;
+    (void)y;
+
+    mfClickX = x - mfMoveX;
+    mfClickY = y - mfMoveY;
 
+
+    double selx,sely;
+    double lon,lat;
+    selx = mfClickX;
+    sely = mfClickY * (-1);
+
+    mpLE_SelX->setText(QString::number(selx,'f',3));
+    mpLE_SelY->setText(QString::number(sely,'f',3));
+
+ //   emit CurrentPosition(selx,sely);
+
+    double x0,y0;
+    GaussProjCal(glon0,glat0,&x0,&y0);
+    GaussProjInvCal(x0+selx,y0+sely,&lon,&lat);
+
+
+    mpLE_SelLon->setText(QString::number(lon,'f',7));
+    mpLE_SelLat->setText(QString::number(lat,'f',7));
 }
 
 void MainWindow::on_actionLoad_triggered()
@@ -271,3 +440,258 @@ void MainWindow::SetCarPos(double x,double y, double fhdg)
 
 }
 
+void MainWindow::onClickSetCarPos()
+{
+    double fLon = mpLE_SelLon->text().toDouble();
+    double fLat = mpLE_SelLat->text().toDouble();
+    double fHeading = mpLE_SelHeading->text().toDouble();
+
+    double x0,y0;
+    GaussProjCal(glon0,glat0,&x0,&y0);
+    double x1,y1;
+    GaussProjCal(fLon,fLat,&x1,&y1);
+
+    double fHdg = (90 - fHeading) * M_PI/180.0;
+    normalhdg(fHdg);
+
+    SetCarPos(x1-x0,y1-y0,fHdg);
+}
+
+void MainWindow::onClickAddPedObj()
+{
+    double fLon = mpLE_SelLon->text().toDouble();
+    double fLat = mpLE_SelLat->text().toDouble();
+    double fHeading = mpLE_SelHeading->text().toDouble();
+
+    double x0,y0;
+    GaussProjCal(glon0,glat0,&x0,&y0);
+    double x1,y1;
+    GaussProjCal(fLon,fLat,&x1,&y1);
+
+    double fHdg = (90 - fHeading) * M_PI/180.0;
+    normalhdg(fHdg);
+
+    AddPedObj(x1-x0,y1-y0,fHdg);
+
+
+}
+
+void MainWindow::onClickAddCarObj()
+{
+    double fLon = mpLE_SelLon->text().toDouble();
+    double fLat = mpLE_SelLat->text().toDouble();
+    double fHeading = mpLE_SelHeading->text().toDouble();
+
+    double x0,y0;
+    GaussProjCal(glon0,glat0,&x0,&y0);
+    double x1,y1;
+    GaussProjCal(fLon,fLat,&x1,&y1);
+
+    double fHdg = (90 - fHeading) * M_PI/180.0;
+    normalhdg(fHdg);
+
+    AddCarObj(x1-x0,y1-y0,fHdg);
+}
+
+void MainWindow::onClickAddCustomObj()
+{
+    double fLon = mpLE_SelLon->text().toDouble();
+    double fLat = mpLE_SelLat->text().toDouble();
+    double fHeading = mpLE_SelHeading->text().toDouble();
+
+    double x0,y0;
+    GaussProjCal(glon0,glat0,&x0,&y0);
+    double x1,y1;
+    GaussProjCal(fLon,fLat,&x1,&y1);
+
+    double fHdg = (90 - fHeading) * M_PI/180.0;
+    normalhdg(fHdg);
+
+    DialogCustomObj xdlgobj;
+    if(xdlgobj.exec() == DialogCustomObj::Accepted)
+    {
+        double fz,flength,fwidth,fheight;
+        xdlgobj.GetValue(fz,flength,fwidth,fheight);
+        AddCustomObj(x1-x0,y1-y0,fz,fHdg,flength,fwidth,fheight);
+    }
+}
+
+void MainWindow::AddPedObj(double x,double y, double fhdg)
+{
+    const double fpedwidth = 0.6;
+    const double fpedlen = 0.6;
+    const double fpedheight = 1.8;
+
+    QPainterPath xpath;
+    double xp[4],yp[4];
+    xp[0] = x + (0 - fpedlen/2.0) * cos(fhdg) - (0 - fpedwidth/2.0) * sin(fhdg);
+    yp[0] = y + (0 - fpedlen/2.0) * sin(fhdg) + (0 - fpedwidth/2.0) * cos(fhdg);
+    xp[1] = x + (0 + fpedlen/2.0) * cos(fhdg) - (0 - fpedwidth/2.0) * sin(fhdg);
+    yp[1] = y + (0 + fpedlen/2.0) * sin(fhdg) + (0 - fpedwidth/2.0) * cos(fhdg);
+    xp[2] = x + (0 + fpedlen/2.0) * cos(fhdg) - (0 + fpedwidth/2.0) * sin(fhdg);
+    yp[2] = y + (0 + fpedlen/2.0) * sin(fhdg) + (0 + fpedwidth/2.0) * cos(fhdg);
+    xp[3] = x + (0 - fpedlen/2.0) * cos(fhdg) - (0 + fpedwidth/2.0) * sin(fhdg);
+    yp[3] = y + (0 - fpedlen/2.0) * sin(fhdg) + (0 + fpedwidth/2.0) * cos(fhdg);
+    xpath.moveTo(xp[0],yp[0]*(-1));
+    xpath.lineTo(xp[1],yp[1]*(-1));
+    xpath.lineTo(xp[2],yp[2]*(-1));
+    xpath.lineTo(xp[3],yp[3]*(-1));
+
+    xpath.closeSubpath();
+    QGraphicsPathItem * pitem = new QGraphicsPathItem;
+    pitem->setPath(xpath);
+
+    pitem->setZValue(99.0);
+    pitem->setBrush(Qt::yellow);
+    pitem->setPen(QPen(Qt::yellow,0.001));
+
+    pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+    mpscene->addItem(pitem);
+
+    AddObjToVector(x,y,0,fhdg,fpedlen,fpedwidth,fpedheight,pitem);
+}
+
+void MainWindow::AddCarObj(double x,double y, double fhdg)
+{
+    const double fcarwidth = 1.9;
+    const double fcarlen = 4.9;
+    const double fcarheight = 1.6;
+
+    QPainterPath xpath;
+    double xp[4],yp[4];
+    xp[0] = x + (0 - fcarlen/2.0) * cos(fhdg) - (0 - fcarwidth/2.0) * sin(fhdg);
+    yp[0] = y + (0 - fcarlen/2.0) * sin(fhdg) + (0 - fcarwidth/2.0) * cos(fhdg);
+    xp[1] = x + (0 + fcarlen/2.0) * cos(fhdg) - (0 - fcarwidth/2.0) * sin(fhdg);
+    yp[1] = y + (0 + fcarlen/2.0) * sin(fhdg) + (0 - fcarwidth/2.0) * cos(fhdg);
+    xp[2] = x + (0 + fcarlen/2.0) * cos(fhdg) - (0 + fcarwidth/2.0) * sin(fhdg);
+    yp[2] = y + (0 + fcarlen/2.0) * sin(fhdg) + (0 + fcarwidth/2.0) * cos(fhdg);
+    xp[3] = x + (0 - fcarlen/2.0) * cos(fhdg) - (0 + fcarwidth/2.0) * sin(fhdg);
+    yp[3] = y + (0 - fcarlen/2.0) * sin(fhdg) + (0 + fcarwidth/2.0) * cos(fhdg);
+    xpath.moveTo(xp[0],yp[0]*(-1));
+    xpath.lineTo(xp[1],yp[1]*(-1));
+    xpath.lineTo(xp[2],yp[2]*(-1));
+    xpath.lineTo(xp[3],yp[3]*(-1));
+
+    xpath.closeSubpath();
+    QGraphicsPathItem * pitem = new QGraphicsPathItem;
+    pitem->setPath(xpath);
+
+    pitem->setZValue(99.0);
+    pitem->setBrush(Qt::yellow);
+    pitem->setPen(QPen(Qt::yellow,0.001));
+
+    pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+    mpscene->addItem(pitem);
+
+    AddObjToVector(x,y,0,fhdg,fcarlen,fcarwidth,fcarheight,pitem);
+
+}
+
+void MainWindow::AddCustomObj(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight)
+{
+    double fcustomwidth = fwidth;
+    double fcustomlen = flen;
+    double fcustomheight = fheight;
+
+    QPainterPath xpath;
+    double xp[4],yp[4];
+    xp[0] = x + (0 - fcustomlen/2.0) * cos(fhdg) - (0 - fcustomwidth/2.0) * sin(fhdg);
+    yp[0] = y + (0 - fcustomlen/2.0) * sin(fhdg) + (0 - fcustomwidth/2.0) * cos(fhdg);
+    xp[1] = x + (0 + fcustomlen/2.0) * cos(fhdg) - (0 - fcustomwidth/2.0) * sin(fhdg);
+    yp[1] = y + (0 + fcustomlen/2.0) * sin(fhdg) + (0 - fcustomwidth/2.0) * cos(fhdg);
+    xp[2] = x + (0 + fcustomlen/2.0) * cos(fhdg) - (0 + fcustomwidth/2.0) * sin(fhdg);
+    yp[2] = y + (0 + fcustomlen/2.0) * sin(fhdg) + (0 + fcustomwidth/2.0) * cos(fhdg);
+    xp[3] = x + (0 - fcustomlen/2.0) * cos(fhdg) - (0 + fcustomwidth/2.0) * sin(fhdg);
+    yp[3] = y + (0 - fcustomlen/2.0) * sin(fhdg) + (0 + fcustomwidth/2.0) * cos(fhdg);
+    xpath.moveTo(xp[0],yp[0]*(-1));
+    xpath.lineTo(xp[1],yp[1]*(-1));
+    xpath.lineTo(xp[2],yp[2]*(-1));
+    xpath.lineTo(xp[3],yp[3]*(-1));
+
+    xpath.closeSubpath();
+    QGraphicsPathItem * pitem = new QGraphicsPathItem;
+    pitem->setPath(xpath);
+
+    pitem->setZValue(99.0);
+    pitem->setBrush(Qt::yellow);
+    pitem->setPen(QPen(Qt::yellow,0.001));
+
+    pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
+    mpscene->addItem(pitem);
+
+    AddObjToVector(x,y,0,fhdg,fcustomlen,fcustomwidth,fcustomheight,pitem);
+
+}
+
+
+void MainWindow::AddObjToVector(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight,QGraphicsPathItem * pitem)
+{
+    iv::simobj xsimobj;
+    xsimobj.mfx = x;
+    xsimobj.mfy = y;
+    xsimobj.mfz = z;
+    xsimobj.mfhdg = fhdg;
+    xsimobj.mflen = flen;
+    xsimobj.mfwidth = fwidth;
+    xsimobj.mfheight = fheight;
+    xsimobj.mpobjitem = pitem;
+    xsimobj.nshowid = mnshowidindex;
+    mnshowidindex++;
+    mvectorsimobj.push_back(xsimobj);
+    mpCBObj->addItem(QString("Obj")+ QString::number(xsimobj.nshowid));
+    mpCBObj->setCurrentIndex(mvectorsimobj.size() -1);
+}
+
+void MainWindow::DelObjFromVector(unsigned int index)
+{
+    if(index>= mvectorsimobj.size())return;
+    iv::simobj xsimobj = mvectorsimobj[index];
+    mpscene->removeItem(xsimobj.mpobjitem);
+    delete xsimobj.mpobjitem;
+    mvectorsimobj.erase(mvectorsimobj.begin() + index);
+}
+
+void MainWindow::onClickDelObj()
+{
+    if(mvectorsimobj.size() == 0)return;
+    unsigned int index = mpCBObj->currentIndex();
+    DelObjFromVector(index);
+    mpCBObj->removeItem(index);
+}
+
+void MainWindow::onCurrentIndexChanged(int index)
+{
+    if(index<0)return;
+    mnSelShowCount = 10;
+    mnSelObj = index;
+}
+
+void MainWindow::onTimerShowSelObj()
+{
+    static bool bShow = true;
+
+    if(mnSelShowCount == 0)return;
+
+    if(bShow)bShow = false;
+    else bShow = true;
+
+    if(mnSelShowCount == 10)bShow = false;
+    if(mnSelShowCount == 1)bShow = true;
+
+    mnSelShowCount--;
+
+    if(mnSelObj<0)return;
+    if(mnSelObj >= static_cast<int>( mvectorsimobj.size()))return;
+
+    iv::simobj xsimobj = mvectorsimobj[mnSelObj];
+
+    xsimobj.mpobjitem->setVisible(bShow);
+
+}
+
+void MainWindow::normalhdg(double & fhdg)
+{
+    while(fhdg < 0)fhdg = fhdg + 2.0*M_PI;
+    while(fhdg > (2.0*M_PI))fhdg = fhdg - 2.0*M_PI;
+}
+

src/tool/simple_planning_simulator/mainwindow.h

@@ -4,6 +4,12 @@
 #include <QMainWindow>
 
 #include <QLabel>
+#include <QGroupBox>
+#include <QLineEdit>
+#include <QSlider>
+#include <QComboBox>
+#include <QPushButton>
+#include <QTimer>
 
 #include "OpenDrive/OpenDrive.h"
 #include "OpenDrive/OpenDriveXmlParser.h"
@@ -14,6 +20,22 @@ QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
 QT_END_NAMESPACE
 
+namespace iv {
+struct simobj
+{
+    double mfx;
+    double mfy;
+    double mfz;
+    double mfhdg;
+    double mflen;
+    double mfwidth;
+    double mfheight;
+    QGraphicsPathItem * mpobjitem;
+    int nshowid;
+};
+
+}
+
 class MainWindow : public QMainWindow
 {
     Q_OBJECT
@@ -27,6 +49,15 @@ private slots:
 
     void onClickXY(double x,double y);
 
+    void onClickSetCarPos();
+    void onClickAddPedObj();
+    void onClickAddCarObj();
+    void onClickAddCustomObj();
+    void onClickDelObj();
+    void onTimerShowSelObj();
+
+    void onCurrentIndexChanged(int index);
+
     void on_actionLoad_triggered();
 
 private:
@@ -44,6 +75,20 @@ private:
 
     QLabel * mpLabel_Status;
 
+    QLineEdit *  mpLE_SelX, * mpLE_SelY, * mpLE_SelLat, * mpLE_SelLon, *mpLE_SelHeading;
+
+    QPushButton * mpPBSetCarPos;
+    QPushButton * mpPBAddPedObj;
+    QPushButton * mpPBAddCarObj;
+    QPushButton * mpPBAddCustomObj;
+
+    QPushButton * mpPBDelObj;
+
+    QComboBox * mpCBObj;
+
+    QTimer * mpTimerShowSelObj;
+
+
     QTabWidget * mTabMain;
     void CreateTab1View(QTabWidget * p);
 
@@ -63,6 +108,16 @@ private:
     std::vector<QGraphicsPathItem *> mvectorviewitem;
     std::vector<QGraphicsPathItem *> mvectorgeoitem;
 
+    double mfClickX,mfClickY;
+
+    double mfMoveX,mfMoveY;
+
+    std::vector<iv::simobj> mvectorsimobj;
+
+    int mnshowidindex = 0;
+    int mnSelObj = 0;
+    int mnSelShowCount = 0;
+
 
 public:
     void resizeEvent(QResizeEvent *event);
@@ -70,6 +125,14 @@ public:
 private:
     void CreateCar();
     void SetCarPos(double x,double y, double fhdg);
+    void AddPedObj(double x,double y, double fhdg);
+    void AddCarObj(double x,double y, double fhdg);
+    void AddCustomObj(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight);
+
+    void AddObjToVector(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight,QGraphicsPathItem * pitem);
+    void DelObjFromVector(unsigned int index);
+
+    void normalhdg(double & fhdg);
 
 };
 #endif // MAINWINDOW_H

src/tool/simple_planning_simulator/simple_planning_simulator.pro

@@ -11,6 +11,7 @@ QMAKE_LFLAGS += -no-pie
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+    ../../common/common/math/gnss_coordinate_convert.cpp \
     ../../common/common/xodr/xodrfunc/roadsample.cpp \
     ../map_lanetoxodr/TinyXML/tinystr.cpp \
     ../map_lanetoxodr/TinyXML/tinyxml.cpp \
@@ -26,10 +27,12 @@ SOURCES += \
     ../map_lanetoxodr/view/roadviewitem.cpp \
     ../map_lanetoxodr/view/viewcreate.cpp \
     ../map_lanetoxodr/view/xodrscenfunc.cpp \
+    dialogcustomobj.cpp \
     main.cpp \
     mainwindow.cpp
 
 HEADERS += \
+    ../../common/common/math/gnss_coordinate_convert.h \
     ../../common/common/xodr/xodrfunc/roadsample.h \
     ../map_lanetoxodr/TinyXML/tinystr.h \
     ../map_lanetoxodr/TinyXML/tinyxml.h \
@@ -40,9 +43,11 @@ HEADERS += \
     ../map_lanetoxodr/view/roadviewitem.h \
     ../map_lanetoxodr/view/viewcreate.h \
     ../map_lanetoxodr/view/xodrscenfunc.h \
+    dialogcustomobj.h \
     mainwindow.h
 
 FORMS += \
+    dialogcustomobj.ui \
     mainwindow.ui
 
 # Default rules for deployment.
@@ -73,9 +78,11 @@ INCLUDEPATH += $$PWD/../../include/msgtype
 
 INCLUDEPATH += $$PWD/../../common/common/xodr
 
+INCLUDEPATH += $$PWD/../../common/common
+
 INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
 
-unix:LIBS += -lboost_thread -lboost_system -lboost_serialization -lprotobuf
+#unix:LIBS += -lboost_thread -lboost_system -lboost_serialization -lprotobuf
 
 RESOURCES += \
     sim.qrc