change simple_planning_simulator, add start and destination icon.

src/tool/simple_planning_simulator/mainwindow.cpp

@@ -8,6 +8,10 @@
 #include <QFileDialog>
 #include <QMessageBox>
 
+#include <functional>
+
+using namespace std;
+
 #include "dialogcustomobj.h"
 
 #define VIEW_WIDTH 1000
@@ -17,6 +21,23 @@
 double glon0 = 117.0866293;
 double glat0 = 39.1364713;
 
+using std::placeholders::_1;
+using std::placeholders::_2;
+using std::placeholders::_3;
+using std::placeholders::_4;
+using std::placeholders::_5;
+
+class xodrobj
+{
+public:
+    double flatsrc;
+    double flonsrc;
+    double fhgdsrc;
+    double flat;
+    double flon;
+    int lane;
+};
+
 MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
     , ui(new Ui::MainWindow)
@@ -82,6 +103,8 @@ MainWindow::MainWindow(QWidget *parent)
 
     //    mpaDecition = iv::modulecomm::RegisterRecvPlus("decition",fundecition);
     mpaDecition = iv::modulecomm::RegisterRecvPlus("deciton",fundecition);
+    mpaXodrDst = iv::modulecomm::RegisterRecvPlus("xodrreq",std::bind(&MainWindow::UpdateXodrObj,this,_1,_2,_3,_4,_5));
+    mpamap = iv::modulecomm::RegisterRecvPlus("tracemap",std::bind(&MainWindow::UpdateMap,this,_1,_2,_3,_4,_5));
 
     setWindowTitle(tr("Simple Planning Simulator"));
 
@@ -261,6 +284,10 @@ void MainWindow::CreateTab1View(QTabWidget * p)
 
     p->addTab(pScroll,"Simulate");
 
+    connect(this,SIGNAL(signalSetDstPos(double,double)),this,SLOT(onSetDstPos(double,double)));
+    connect(this,SIGNAL(signalSetStartPos(double,double)),this,SLOT(onSetStartPos(double,double)));
+    connect(this,SIGNAL(signalUpdateTraceMap()),this,SLOT(onUpdateTraceMap()));
+
 
 
 }
@@ -544,6 +571,7 @@ void MainWindow::CreateEP()
     mpscene->addItem(ppix);
     ppix->setZValue(101.0);
     mppixep = ppix;
+    mppixep->setVisible(false);
 }
 
 void MainWindow::SetCarPos(double x,double y, double fhdg)
@@ -884,6 +912,76 @@ void MainWindow::onTimerUpdateCarPos()
     mppixcar->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + x - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - y + y1);
 
     if(mbTrack)myview->centerOn(mfViewMoveX +VIEW_WIDTH/2.0 + x - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - y + y1);
+}
+
+void MainWindow::onSetDstPos(double x, double y)
+{
+    double fViewSize = 2.0;
+    double x1,y1;
+    x1 = fViewSize * 0.5;
+    y1 = -fViewSize;
+    mppixep->setVisible(true);
+    mppixep->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + x - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - y + y1);
+}
+
+void MainWindow::onSetStartPos(double x, double y)
+{
+    double fViewSize = 2.0;
+    double x1,y1;
+    x1 = fViewSize * 0.5;
+    y1 = -fViewSize;
+    mppixsp->setVisible(true);
+    mppixsp->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + x - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - y + y1);
+}
+
+void MainWindow::onUpdateTraceMap()
+{
+    std::vector<iv::GPS_INS> xvectortracemap;
+    mmutexmap.lock();
+    xvectortracemap = mvectormap;
+    mmutexmap.unlock();
+
+    double x0,y0;
+    double x,y;
+
+    if(mxodr.GetHeader() == NULL)
+    {
+        std::cout<<" onUpdateTraceMap , xodr fail."<<std::endl;
+        return;
+    }
+
+    double lon0,lat0;
+    mxodr.GetHeader()->GetLat0Lon0(lat0,lon0);
+    GaussProjCal(lon0,lat0,&x0,&y0);
+
+    int i;
+
+    while(mvectormappointitem.size()>0)
+    {
+        QGraphicsEllipseItem * pitem = mvectormappointitem[0];
+        mpscene->removeItem(pitem);
+        delete pitem;
+        mvectormappointitem.erase(mvectormappointitem.begin());
+    }
+
+    int nPointSize = static_cast<int>(xvectortracemap.size());
+    for(i=0;i<nPointSize;i++)
+    {
+        GaussProjCal(xvectortracemap[i].gps_lng,xvectortracemap[i].gps_lat,&x,&y);
+        x = x - x0;
+        y = y - y0;
+        double fpointwidth = 0.1;
+        QGraphicsEllipseItem * pitem = new QGraphicsEllipseItem(0.0,0.0,fpointwidth,fpointwidth);
+        pitem->setBrush(Qt::green);
+        pitem->setPen(QPen(Qt::yellow,0.001));
+        mpscene->addItem(pitem);
+        mvectormappointitem.push_back(pitem);
+        pitem->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + x  - fpointwidth * 0.5 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - y - fpointwidth * 0.5);
+    }
+
+
+
+
 }
 
 void MainWindow::UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -903,6 +1001,94 @@ void MainWindow::UpdateDecition(const char * strdata,const unsigned int nSize,co
         mpsimmodel->SetCMD(xdecition.accelerator(),xdecition.torque(),xdecition.brake(),xdecition.wheelangle());
 }
 
+void MainWindow::UpdateXodrObj(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    if(nSize<sizeof(xodrobj))
+    {
+        std::cout<<"UpdateXodrObj small."<<std::endl;
+        return;
+    }
+
+    xodrobj xo;
+    memcpy(&xo,strdata,sizeof(xodrobj));
+
+    double x0,y0;
+    double x,y;
+
+    if(mxodr.GetHeader() == NULL)
+    {
+        std::cout<<" UpdateXodrObj , xodr fail."<<std::endl;
+        return;
+    }
+
+    double lon0,lat0;
+    mxodr.GetHeader()->GetLat0Lon0(lat0,lon0);
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    GaussProjCal(xo.flon,xo.flat,&x,&y);
+    x = x - x0;
+    y = y - y0;
+
+//    emit signalSetDstPos(x,y);
+
+}
+
+void MainWindow::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    if(nSize<sizeof(iv::GPS_INS))
+    {
+        std::cout<<" no map data."<<std::endl;
+        return;
+    }
+
+    int nPointSize = static_cast<int>(nSize/sizeof(iv::GPS_INS));
+    iv::GPS_INS * pmap = (iv::GPS_INS * )strdata;
+
+    double x0,y0;
+    double x,y;
+
+    if(mxodr.GetHeader() == NULL)
+    {
+        std::cout<<" UpdateMap , xodr fail."<<std::endl;
+        return;
+    }
+
+    double lon0,lat0;
+    mxodr.GetHeader()->GetLat0Lon0(lat0,lon0);
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    GaussProjCal(pmap[0].gps_lng,pmap[0].gps_lat,&x,&y);
+    x = x - x0;
+    y = y - y0;
+
+    emit signalSetStartPos(x,y);
+
+
+    GaussProjCal(pmap[nPointSize -1].gps_lng,pmap[nPointSize -1].gps_lat,&x,&y);
+    x = x - x0;
+    y = y - y0;
+
+    emit signalSetDstPos(x,y);
+
+    mmutexmap.lock();
+    mvectormap.clear();
+    int i;
+    for(i=0;i<nPointSize;i++)
+    {
+        mvectormap.push_back(pmap[i]);
+    }
+    mmutexmap.unlock();
+
+    emit signalUpdateTraceMap();
+
+
+}
 
 void MainWindow::on_actionSaveObject_triggered()
 {

src/tool/simple_planning_simulator/mainwindow.h

@@ -27,6 +27,87 @@ namespace Ui { class MainWindow; }
 QT_END_NAMESPACE
 
 
+namespace iv {
+struct GPS_INS
+{
+    int valid = 0xff;
+    int index = 0;	//gps点序号
+
+    double gps_lat = 0;//纬度
+    double gps_lng = 0;//经度
+
+    double gps_x = 0;
+    double gps_y = 0;
+    double gps_z = 0;
+
+    double frenet_s=0;
+    double frenet_s_dot=0;
+    double frenet_s_dot_dot=0;
+    double frenet_d=0;
+    double frenet_d_dot=0;
+    double frenet_d_dot_dot=0;
+
+    double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+    double ins_pitch_angle = 0;	//俯仰角
+    double ins_heading_angle = 0;	//航向角
+
+    int ins_status = 0;	//惯导状态 4
+    int rtk_status = 0;	//rtk状态 6 -5 -3
+    int gps_satelites_num = 0;
+
+    //-----加速度--------------
+    double accel_x = 0;
+    double accel_y = 0;
+    double accel_z = 0;
+
+    //-------角速度------------
+    double ang_rate_x = 0;
+    double ang_rate_y = 0;
+    double ang_rate_z = 0;
+
+    //-----------方向速度--------------
+    double vel_N = 0;
+    double vel_E = 0;
+    double vel_D = 0;
+
+    int speed_mode = 0;
+    int mode2 = 0;
+    double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+    int roadMode;
+    int runMode;
+    int roadSum;
+    int roadOri;
+
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
+
+    bool mbInLaneAvoid = false; //if true suport In Lane Avoid
+    double gps_lat_avoidleft;
+    double gps_lng_avoidleft;
+    double gps_lat_avoidright;
+    double gps_lng_avoidright;
+    double gps_x_avoidleft = 0;
+    double gps_y_avoidleft = 0;
+    double gps_x_avoidright = 0;
+    double gps_y_avoidright = 0;
+
+    bool mbnoavoid = false; //If true this point is no avoid.
+    double mfCurvature = 0.0;
+
+    char mcreserved[10];
+    int mnreserved[5];
+    double mfreserved[2];
+
+
+
+};
+}
+
 
 class MainWindow : public QMainWindow
 {
@@ -36,6 +117,11 @@ public:
     MainWindow(QWidget *parent = nullptr);
     ~MainWindow();
 
+signals:
+    void signalSetDstPos(double x, double y);
+    void signalSetStartPos(double x, double y);
+    void signalUpdateTraceMap();
+
 private slots:
     virtual void paintEvent(QPaintEvent *);
 
@@ -64,6 +150,10 @@ private slots:
 
     void on_actionTrack_triggered();
 
+    void onSetDstPos(double x, double y);
+    void onSetStartPos(double x, double y);
+    void onUpdateTraceMap();
+
 private:
     Ui::MainWindow *ui;
 
@@ -140,6 +230,18 @@ private:
     simmodel * mpsimmodel;
 
     void * mpaDecition;
+    void * mpaXodrDst;
+    void * mpamap;
+
+    bool mbSetDst;
+    double mfx_dst,mfy_dst;
+
+    std::vector<iv::GPS_INS> mvectormap;
+    std::mutex mmutexmap;
+
+
+    std::vector<QGraphicsEllipseItem *> mvectormappointitem;
+
 
 
 public:
@@ -160,6 +262,8 @@ private:
     void normalhdg(double & fhdg);
 
     void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateXodrObj(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
 };
 #endif // MAINWINDOW_H

src/tool/simple_planning_simulator/mainwindow.ui

@@ -15,7 +15,7 @@
   </property>
   <property name="windowIcon">
    <iconset resource="sim.qrc">
-    <normaloff>:/car.png</normaloff>:/car.png</iconset>
+    <normaloff>:/simicon.png</normaloff>:/simicon.png</iconset>
   </property>
   <widget class="QWidget" name="centralwidget"/>
   <widget class="QMenuBar" name="menubar">

src/tool/simple_planning_simulator/sim.qrc

@@ -3,5 +3,6 @@
         <file>car.png</file>
         <file>ep.png</file>
         <file>sp.png</file>
+        <file>simicon.png</file>
     </qresource>
 </RCC>