change simple_planning_simulator. complete.

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -79,5 +79,7 @@ INCLUDEPATH += $$PWD/../../include/msgtype
 INCLUDEPATH += /usr/include/opencv4/
 #LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
 
+LIBS += /usr/lib/x86_64-linux-gnu/libopencv*.so
 
-LIBS += /usr/lib/libopencv*.so
+
+#LIBS += /usr/lib/libopencv*.so

src/tool/simple_planning_simulator/mainwindow.cpp

@@ -75,6 +75,8 @@ MainWindow::MainWindow(QWidget *parent)
     //    mpaDecition = iv::modulecomm::RegisterRecvPlus("decition",fundecition);
     mpaDecition = iv::modulecomm::RegisterRecvPlus("deciton",fundecition);
 
+    setWindowTitle(tr("Simple Planning Simulator"));
+
 
 }
 
@@ -101,10 +103,10 @@ void MainWindow::CreateTab1View(QTabWidget * p)
     QGroupBox * pGroup = new QGroupBox();
     pGroup->setGeometry(0,0,mnFontHeight * 21,mnFontHeight * 160);
 
-    QLabel * pLabel;
+ //   QLabel * pLabel;
     QLineEdit * pLE;
     QPushButton * pPB;
-    QSlider * pSlider;
+ //   QSlider * pSlider;
     QComboBox * pCB;
     QCheckBox * pCheck;
 
@@ -113,7 +115,7 @@ void MainWindow::CreateTab1View(QTabWidget * p)
     int nXPos = 10;
     int nYPos = 30;
 
-    int i;
+ //   int i;
 
     int nSpace = mnFontHeight * 65/10;
     int nLEWidth = mnFontHeight * 6;
@@ -129,7 +131,7 @@ void MainWindow::CreateTab1View(QTabWidget * p)
     pLE->setText("");
     pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
     mpLE_SelY = pLE;
-    nXPos = nXPos + nSpace;
+ //   nXPos = nXPos + nSpace;
 
 
 
@@ -246,10 +248,6 @@ void MainWindow::CreateTab1View(QTabWidget * p)
     mpCBSim = pCheck;
     connect(mpCBSim,SIGNAL(stateChanged(int)),this,SLOT(onSimStateChanged(int)));
 
-
-
-
-
     QScrollArea * pScroll = new QScrollArea();
     pScroll->setWidget(pGroup);
 
@@ -356,7 +354,7 @@ void MainWindow::on_actionLoad_triggered()
 }
 
 
-#include <QtGui>
+//#include <QtGui>
 
 void MainWindow::UpdateScene()
 {
@@ -649,6 +647,7 @@ void MainWindow::AddCarObj(double x,double y, double fhdg)
 
 void MainWindow::AddCustomObj(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight)
 {
+    (void)z;
     double fcustomwidth = fwidth;
     double fcustomlen = flen;
     double fcustomheight = fheight;
@@ -700,6 +699,9 @@ void MainWindow::AddObjToVector(double x,double y, double z,double fhdg,double f
     mvectorsimobj.push_back(xsimobj);
     mpCBObj->addItem(QString("Obj")+ QString::number(xsimobj.nshowid));
     mpCBObj->setCurrentIndex(mvectorsimobj.size() -1);
+
+    mpsimmodel->AddSimobj(xsimobj);
+
 }
 
 void MainWindow::DelObjFromVector(unsigned int index)
@@ -709,6 +711,8 @@ void MainWindow::DelObjFromVector(unsigned int index)
     mpscene->removeItem(xsimobj.mpobjitem);
     delete xsimobj.mpobjitem;
     mvectorsimobj.erase(mvectorsimobj.begin() + index);
+
+    mpsimmodel->DelSimobj(index);
 }
 
 void MainWindow::onClickDelObj()
@@ -787,10 +791,12 @@ void MainWindow::onSimStateChanged(int nState)
 {
     if(nState == Qt::Checked)
     {
+        mbSimulate = true;
         mpCBShift->setEnabled(false);
     }
     else
     {
+        mbSimulate  = false;
         mpCBShift->setEnabled(true);
     }
 }
@@ -819,7 +825,9 @@ void MainWindow::onTimerUpdateCarPos()
 
 void MainWindow::UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-
+    (void)index;
+    (void)dt;
+    (void)strmemname;
     iv::brain::decition xdecition;
     if(!xdecition.ParseFromArray(strdata,nSize))
     {
@@ -828,6 +836,92 @@ void MainWindow::UpdateDecition(const char * strdata,const unsigned int nSize,co
     }
     //ServiceCarStatus.mfAcc = xdecition.accelerator();   //
 
-    mpsimmodel->SetCMD(xdecition.accelerator(),xdecition.torque(),xdecition.brake(),xdecition.wheelangle());
+    if(mbSimulate)
+        mpsimmodel->SetCMD(xdecition.accelerator(),xdecition.torque(),xdecition.brake(),xdecition.wheelangle());
+}
+
+
+void MainWindow::on_actionSaveObject_triggered()
+{
+    QString str = QFileDialog::getSaveFileName(this,"Save Object",".","*.xml");
+    if(str.isEmpty())return;
+
+    if(str.right(4) != ".xml")str.append(".xml");
+
+    TiXmlDocument doc;
+
+    TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );
+    doc.LinkEndChild( decl );
+
+    TiXmlElement *rootNode = new TiXmlElement("Objects");
+    doc.LinkEndChild(rootNode);
+
+    // Write objects
+    unsigned int nobjsize = mvectorsimobj.size();
+    for(unsigned int i=0; i<nobjsize; i++)
+    {
+        iv::simobj xsimobj = mvectorsimobj[i];
+        TiXmlElement *nodeObj = new TiXmlElement("Object");
+        rootNode->LinkEndChild(nodeObj);
+
+        nodeObj->SetDoubleAttribute("x",xsimobj.mfx);
+        nodeObj->SetDoubleAttribute("y",xsimobj.mfy);
+        nodeObj->SetDoubleAttribute("z",xsimobj.mfz);
+        nodeObj->SetDoubleAttribute("hdg",xsimobj.mfhdg);
+        nodeObj->SetDoubleAttribute("length",xsimobj.mflen);
+        nodeObj->SetDoubleAttribute("width",xsimobj.mfwidth);
+        nodeObj->SetDoubleAttribute("height",xsimobj.mfheight);
+    }
+
+    doc.SaveFile( str.toStdString() );
+}
+
+
+void MainWindow::on_actionLoadObject_triggered()
+{
+    QString str = QFileDialog::getOpenFileName(this,"Save Object",".","*.xml");
+    if(str.isEmpty())return;
+
+    std::vector<iv::simobj> xvectorsimobj;
+
+    TiXmlDocument doc( str.toStdString() );
+    bool loadOkay = doc.LoadFile();
+    if (loadOkay)
+    {
+        TiXmlElement *rootNode=doc.FirstChildElement();
+
+        TiXmlElement *node = rootNode->FirstChildElement("Object");
+        while (node!=0)
+        {
+            int checker=TIXML_SUCCESS;
+            iv::simobj xsimobj;
+            checker+=node->QueryDoubleAttribute("x",&xsimobj.mfx);
+            checker+=node->QueryDoubleAttribute("y",&xsimobj.mfy);
+            checker+=node->QueryDoubleAttribute("z",&xsimobj.mfz);
+            checker+=node->QueryDoubleAttribute("hdg",&xsimobj.mfhdg);
+            checker+=node->QueryDoubleAttribute("length",&xsimobj.mflen);
+            checker+=node->QueryDoubleAttribute("width",&xsimobj.mfwidth);
+            checker+=node->QueryDoubleAttribute("height",&xsimobj.mfheight);
+
+            if(checker == TIXML_SUCCESS)
+            {
+                xvectorsimobj.push_back(xsimobj);
+            }
+
+            node=node->NextSiblingElement("Object");
+        }
+    }
+
+    if(xvectorsimobj.size() > 0)
+    {
+        while(mvectorsimobj.size()>0)DelObjFromVector(0);
+        unsigned int i;
+        unsigned int nObjSize = xvectorsimobj.size();
+        for(i=0;i<nObjSize;i++)
+        {
+            AddCustomObj(xvectorsimobj[i].mfx,xvectorsimobj[i].mfy,xvectorsimobj[i].mfz,xvectorsimobj[i].mfhdg,xvectorsimobj[i].mflen,xvectorsimobj[i].mfwidth,
+                         xvectorsimobj[i].mfheight);
+        }
+    }
 }
 

src/tool/simple_planning_simulator/mainwindow.h

@@ -26,21 +26,7 @@ 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
 {
@@ -72,6 +58,10 @@ private slots:
 
     void onTimerUpdateCarPos();
 
+    void on_actionSaveObject_triggered();
+
+    void on_actionLoadObject_triggered();
+
 private:
     Ui::MainWindow *ui;
 
@@ -118,6 +108,8 @@ private:
 
     int mnMeasure = 0;
 
+    bool mbSimulate = false;
+
 
 private:
     int mnFontHeight;

src/tool/simple_planning_simulator/mainwindow.ui

@@ -32,6 +32,8 @@
      <string>File</string>
     </property>
     <addaction name="actionLoad"/>
+    <addaction name="actionSaveObject"/>
+    <addaction name="actionLoadObject"/>
    </widget>
    <widget class="QMenu" name="menuTool">
     <property name="title">
@@ -53,6 +55,16 @@
     <string>Measuring Distance</string>
    </property>
   </action>
+  <action name="actionSaveObject">
+   <property name="text">
+    <string>SaveObject</string>
+   </property>
+  </action>
+  <action name="actionLoadObject">
+   <property name="text">
+    <string>LoadObject</string>
+   </property>
+  </action>
  </widget>
  <resources>
   <include location="sim.qrc"/>

src/tool/simple_planning_simulator/simmodel.cpp

@@ -14,6 +14,7 @@ simmodel::simmodel()
     mvel = 0;
 
     mpa = iv::modulecomm::RegisterSend("hcp2_gpsimu",100000,1);
+    mpatrack = iv::modulecomm::RegisterSend("lidar_track",1000000,1);
 
     mpthread = new std::thread(&simmodel::threadupdate,this);
     mpthreadstate = new std::thread(&simmodel::threadstate,this);
@@ -191,8 +192,118 @@ void simmodel::threadstate()
         }
         delete[] strser;
 
+
+        std::vector<iv::simobj> xvectorsimobj;
+        mmutexsimobj.lock();
+        xvectorsimobj = mvectorsimobj;
+        mmutexsimobj.unlock();
+
+        iv::lidar::objectarray xobjarray;
+        unsigned int nobjsize = xvectorsimobj.size();
+        unsigned int i;
+        for(i=0;i<nobjsize;i++)
+        {
+            iv::simobj xsimobj = xvectorsimobj[i];
+            double x_obj,y_obj,hdg_obj;
+            double fdis = sqrt(pow(xsimobj.mfx - mx,2)+ pow(xsimobj.mfy - my,2));
+            if(fdis <= mfSensorRange)
+            {
+                double frothdg = 0.0 - mhdg + M_PI/2.0;
+                double xraw,yraw;
+                xraw = xsimobj.mfx - mx;
+                yraw = xsimobj.mfy - my;
+                x_obj = xraw * cos(frothdg) - yraw * sin(frothdg);//fdis * cos(frothdg);
+                y_obj = xraw * sin(frothdg) + yraw * cos(frothdg);//fdis * sin(frothdg);
+                hdg_obj = xsimobj.mfhdg + frothdg;
+                iv::lidar::lidarobject xlidarobj;
+                xlidarobj.set_pose_reliable(true);
+                xlidarobj.set_velocity_reliable(true);
+                xlidarobj.set_velocity_linear_x(mvel*(-1));
+                xlidarobj.set_acceleration_linear_y(0);
+                xlidarobj.set_angle(hdg_obj);
+                xlidarobj.set_tyaw(hdg_obj);
+                iv::lidar::PointXYZ xpos;
+                xpos.set_x(x_obj);
+                xpos.set_y(y_obj);
+                xpos.set_z(-1.8);
+                iv::lidar::PointXYZ * plx;
+                plx = xlidarobj.mutable_centroid();
+                plx->CopyFrom(xpos);
+
+                iv::lidar::Dimension ld;
+                iv::lidar::Dimension * pld;
+                ld.set_x(xsimobj.mflen);
+                ld.set_y(xsimobj.mfwidth);
+                ld.set_z(xsimobj.mfheight);
+                //        lidarobj.dimensions.x = ((length < 0) ? -1 * length : length);
+                //        lidarobj.dimensions.y = ((width < 0) ? -1 * width : width);
+                //        lidarobj.dimensions.z = ((height < 0) ? -1 * height : height);
+                pld = xlidarobj.mutable_dimensions();
+                pld->CopyFrom(ld);
+                xlidarobj.set_mntype(0);
+                iv::lidar::lidarobject * pobj =  xobjarray.add_obj();
+                pobj->CopyFrom(xlidarobj);
+            }
+
+        }
+
+        if(xobjarray.obj_size() == 0)  //Add a fake obj to array, tell ui, lidar is ok
+        {
+            iv::lidar::lidarobject xlidarobj;
+            xlidarobj.set_pose_reliable(true);
+            xlidarobj.set_velocity_reliable(true);
+            xlidarobj.set_velocity_linear_x(mvel*(-1));
+            xlidarobj.set_acceleration_linear_y(0);
+            xlidarobj.set_angle(0);
+            xlidarobj.set_tyaw(0);
+            iv::lidar::PointXYZ xpos;
+            xpos.set_x(-1000);
+            xpos.set_y(-1000);
+            xpos.set_z(-1.8);
+            iv::lidar::PointXYZ * plx;
+            plx = xlidarobj.mutable_centroid();
+            plx->CopyFrom(xpos);
+
+            iv::lidar::Dimension ld;
+            iv::lidar::Dimension * pld;
+            ld.set_x(0.1);
+            ld.set_y(0.1);
+            ld.set_z(0.1);
+            //        lidarobj.dimensions.x = ((length < 0) ? -1 * length : length);
+            //        lidarobj.dimensions.y = ((width < 0) ? -1 * width : width);
+            //        lidarobj.dimensions.z = ((height < 0) ? -1 * height : height);
+            pld = xlidarobj.mutable_dimensions();
+            pld->CopyFrom(ld);
+            xlidarobj.set_mntype(0);
+            iv::lidar::lidarobject * pobj =  xobjarray.add_obj();
+            pobj->CopyFrom(xlidarobj);
+        }
+
+        std::string out = xobjarray.SerializeAsString();
+        //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
+        iv::modulecomm::ModuleSendMsg(mpatrack,out.data(),out.length());
+
         std::this_thread::sleep_for(std::chrono::milliseconds(20));
     }
 }
 
+void simmodel::AddSimobj(iv::simobj & xsimobj)
+{
+    mmutexsimobj.lock();
+    mvectorsimobj.push_back(xsimobj);
+    mmutexsimobj.unlock();
+}
+
+void simmodel::DelSimobj(unsigned int index)
+{
+    if(index>=(static_cast<unsigned int>(mvectorsimobj.size())))
+    {
+        return;
+    }
+
+    mmutexsimobj.lock();
+    mvectorsimobj.erase(mvectorsimobj.begin() + index);
+    mmutexsimobj.unlock();
+}
+
 

src/tool/simple_planning_simulator/simmodel.h

@@ -5,14 +5,36 @@
 #include <thread>
 
 #include "gpsimu.pb.h"
+#include "objectarray.pb.h"
 
 #include "modulecomm.h"
 
+#include <QGraphicsItem>
+
+#include <mutex>
+
+
+namespace iv {
+struct simobj
+{
+    double mfx;
+    double mfy;
+    double mfz;
+    double mfhdg;
+    double mflen;
+    double mfwidth;
+    double mfheight;
+    QGraphicsPathItem * mpobjitem;
+    int nshowid;
+};
+
+}
+
 class simmodel
 {
 public:
     simmodel();
-    ~simmodel();
+    virtual ~simmodel();
 
     enum SHIFT_GEER
     {
@@ -34,6 +56,8 @@ protected:
 
     bool mbInitOK = false;
 
+    double mfSensorRange = 50.0;
+
 public:
     double GetX();
     double GetY();
@@ -65,6 +89,9 @@ public:
     void Setvel(double fvel);
     void SetLon0Lat0(double flon0,double flat0);
 
+    void AddSimobj(iv::simobj & xsimobj);
+    void DelSimobj(unsigned int index);
+
 
 private:
     std::thread * mpthread;
@@ -75,6 +102,10 @@ private:
     void threadstate();
 
     void * mpa;
+    void * mpatrack;
+
+    std::vector<iv::simobj> mvectorsimobj;
+    std::mutex mmutexsimobj;
 
 
 };

src/tool/simple_planning_simulator/simmodel_shenlan.cpp

@@ -8,6 +8,11 @@ simmodel_shenlan::simmodel_shenlan()
     mbInitOK = true;
 }
 
+simmodel_shenlan::~simmodel_shenlan()
+{
+
+}
+
 void simmodel_shenlan::CalcModel()
 {
     int64_t ncmdtime = mlastcmdtime;
@@ -15,7 +20,7 @@ void simmodel_shenlan::CalcModel()
     int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
     if((abs(nnow - ncmdtime)/1000000)>maxnocmdtime)
     {
-        std::cout<<" more than 1 seconds no cmd. not calc."<<std::endl;
+//        std::cout<<" more than 1 seconds no cmd. not calc."<<std::endl;
         mvel = 0;
         mwheelsteer = 0;
         return;
@@ -61,7 +66,7 @@ void simmodel_shenlan::CalcModel()
         double facc;
 
         double fVehWeight = 1800;
-        double fg = 9.8;
+//        double fg = 9.8;
         double fRollForce = 50;
         const double fRatio = 2.5;
 

src/tool/simple_planning_simulator/simmodel_shenlan.h

@@ -7,6 +7,7 @@ class simmodel_shenlan : public simmodel
 {
 public:
     simmodel_shenlan();
+    virtual ~simmodel_shenlan();
 
     void CalcModel();
 

src/tool/simple_planning_simulator/simple_planning_simulator.pro

@@ -15,6 +15,8 @@ SOURCES += \
     ../../common/common/xodr/xodrfunc/roadsample.cpp \
     ../../include/msgtype/decition.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/object.pb.cc \
+    ../../include/msgtype/objectarray.pb.cc \
     ../map_lanetoxodr/TinyXML/tinystr.cpp \
     ../map_lanetoxodr/TinyXML/tinyxml.cpp \
     ../map_lanetoxodr/TinyXML/tinyxmlerror.cpp \
@@ -40,6 +42,8 @@ HEADERS += \
     ../../common/common/xodr/xodrfunc/roadsample.h \
     ../../include/msgtype/decition.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h \
     ../map_lanetoxodr/TinyXML/tinystr.h \
     ../map_lanetoxodr/TinyXML/tinyxml.h \
     ../map_lanetoxodr/function/circlefitting.h \