change simple_planning_simulator, add xml and weather, complete steer view, signal not complete.

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -106,7 +106,7 @@ HEADERS += \
     ../../include/msgtype/groupmsg.pb.h \
     ../../include/msgtype/brainrunstate.pb.h
 
-#DEFINES += USEAUTOWAREMPC
+DEFINES += USEAUTOWAREMPC
 
 
 if(contains(DEFINES,USEAUTOWAREMPC)){

src/tool/simple_planning_simulator/mainwindow.cpp

@@ -38,6 +38,8 @@ public:
     int lane;
 };
 
+static QString gWeather[] = {"晴天","雨天","大雾","下雪","夜晚"};
+
 MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
     , ui(new Ui::MainWindow)
@@ -84,8 +86,8 @@ MainWindow::MainWindow(QWidget *parent)
 
     mTabMain = p;
 
-    mpTabVehState = new QTabWidget(ui->centralwidget);
-    mpTabCmd = new QTabWidget(ui->centralwidget);
+ //   mpTabVehState = new QTabWidget(ui->centralwidget);
+ //   mpTabCmd = new QTabWidget(ui->centralwidget);
 
     CreateCar();
     CreateSP();
@@ -100,8 +102,14 @@ MainWindow::MainWindow(QWidget *parent)
     connect(mpTimerShowSelObj,SIGNAL(timeout()),this,SLOT(onTimerShowSelObj()));
     mpTimerShowSelObj->start(100);
 
+    LoadXML();
     mpsimmodel = new simmodel_shenlan();
     mpsimmodel->SetLon0Lat0(glon0,glat0);
+    mpsimmodel->SetSensorRange(mfSensorRange);
+    mpsimmodel->SetWheelMaxSpeed(mfWHEELSPEED);
+    mpsimmodel->SetWheelBase(mfwheelbase);
+    mpsimmodel->SetWheelRatio(mfwheelratio);
+    mpsimmodel->SetVehWeight(mfVehWeight);
 
     mpTimerUpdateCarPos = new QTimer(this);
     connect(mpTimerUpdateCarPos,SIGNAL(timeout()),this,SLOT(onTimerUpdateCarPos()));
@@ -133,12 +141,12 @@ void MainWindow::resizeEvent(QResizeEvent *event)
     int nkeep = 110;
     int nbase = sizemain.height() - nkeep;
     if(nbase < 0) nbase = 30;
-    double fratio1 = 0.65;
+    double fratio1 = 0.95;
     double fratio2 = 0.25;
     double fratio3 = 0.10;
     mTabMain->setGeometry(sizemain.width()-mnFontHeight * 22,30,mnFontHeight * 22,nbase * fratio1);
-    mpTabVehState->setGeometry(sizemain.width()-mnFontHeight * 22,60 + nbase * fratio1,mnFontHeight * 22,nbase * fratio2);
-    mpTabCmd->setGeometry(sizemain.width()-mnFontHeight * 22,90 + nbase * (fratio1 + fratio2),mnFontHeight * 22, nbase * fratio3);
+//    mpTabVehState->setGeometry(sizemain.width()-mnFontHeight * 22,60 + nbase * fratio1,mnFontHeight * 22,nbase * fratio2);
+//    mpTabCmd->setGeometry(sizemain.width()-mnFontHeight * 22,90 + nbase * (fratio1 + fratio2),mnFontHeight * 22, nbase * fratio3);
 
 }
 
@@ -149,7 +157,7 @@ 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;
@@ -303,6 +311,26 @@ void MainWindow::CreateTab1View(QTabWidget * p)
     connect(this,SIGNAL(signalSetStartPos(double,double)),this,SLOT(onSetStartPos(double,double)));
     connect(this,SIGNAL(signalUpdateTraceMap()),this,SLOT(onUpdateTraceMap()));
 
+    nYPos = nYPos + mnFontHeight * 2;
+
+    nXPos = 10;
+    nYPos = nYPos + mnFontHeight * 2;
+
+    pLabel = new QLabel(pGroup);
+    pLabel->setText(tr("天气"));
+    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight*3/2);
+
+    nXPos = nXPos + nSpace;
+
+    pCB = new QComboBox(pGroup);
+    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight*3/2);
+    int i;
+    for(i=0;i<5;i++)
+    {
+        pCB->addItem(gWeather[i]);
+    }
+
+
 
 
 }
@@ -336,6 +364,8 @@ void MainWindow::onMyViewMoveOrScale()
     myview->GetPosXY(pos_x,pos_y,view_x,view_y);
     std::cout<<"view x: "<<view_x<<" view y: "<<view_y<<std::endl;
     mppixsteer->setPos(view_x  ,view_y );
+    mfsteerposx = view_x;
+    mfsteerposy = view_y;
 
     if(beishu == 0.0)return;
     mpitemstate->setScale(1.0/beishu);
@@ -642,6 +672,8 @@ void MainWindow::CreateSteer()
     {
         fscale = fscale/beishu;
     }
+    mbeishu = beishu;
+    mfScale = fscale;
     ppix->setRotation(0);
     ppix->setScale(fscale);
     double pos_x,pos_y;
@@ -649,7 +681,14 @@ void MainWindow::CreateSteer()
     pos_y = 10;
     double view_x,view_y;
     myview->GetPosXY(view_x,view_y,pos_x,pos_y);
-    ppix->setPos(view_x,view_y);
+    double fviewoffx,fviewoffy;
+    fviewoffx = fViewSize *0.5 /beishu;
+    fviewoffy = fviewoffx;
+    ppix->setPos(view_x ,view_y );
+    mfsteerposx = view_x;
+    mfsteerposy = view_y;
+
+   //   ppix->setOffset(fviewoffx,fviewoffy );
     mpscene->addItem(ppix);
     ppix->setZValue(101.0);
     mppixsteer = ppix;
@@ -667,6 +706,7 @@ void MainWindow::CreateStateItem()
     font.setUnderline(true);  // 下划线
     pState->setFont(font);
     pState->setBrush(QBrush(QColor(0, 160, 230)));
+    pState->setZValue(101.0);
     mpscene->addItem(pState);
     mpitemstate = pState;
 }
@@ -683,6 +723,7 @@ void MainWindow::CreateCMDItem()
     font.setUnderline(true);  // 下划线
     pState->setFont(font);
     pState->setBrush(QBrush(QColor(0, 160, 230)));
+    pState->setZValue(101.0);
     mpscene->addItem(pState);
     mpitemcmd = pState;
 }
@@ -1021,10 +1062,64 @@ void MainWindow::onTimerUpdateCarPos()
 
     mppixcar->setRotation(fhead);
 
+    double fwheelangle = mpsimmodel->Getwheelsteer() * 13.6 * 180.0/M_PI;
+    mfWheelAngle = fwheelangle;
+
+    char strstate[1000];
+    snprintf(strstate,1000,"Vehicle State vel:%6.3fkm/h acc:%6.1fm/s2 steer:%6.1f°",mpsimmodel->Getvel()*3.6,mpsimmodel->Getacc(),
+             fwheelangle);
+    mpitemstate->setText(strstate);
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    if(abs(nnow - mnlastdecitiontime)< 1e9)
+    {
+        iv::brain::decition xdecition;
+        mmutexdecition.lock();
+        xdecition.CopyFrom(mdecition);
+        mmutexdecition.unlock();
+        snprintf(strstate,1000,"Command torque:%6.3f  brake:%6.3f  wheel:%6.3f",
+                 xdecition.torque(),xdecition.brake(),xdecition.wheelangle());
+        mpitemcmd->setText(strstate);
+    }
+
+    mppixsteer->setRotation(fwheelangle *(-1) );
+
+    double beishu = myview->Getbeishu();
+
+
+
 
     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);
+    if(mbTrack)
+    {
+        myview->centerOn(mfViewMoveX +VIEW_WIDTH/2.0 + x - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - y + y1);
+        double fViewSize = mfsteerviewsize;
+        double pos_x,pos_y;
+        pos_x = 10;
+        pos_y = 10;
+        double view_x,view_y;
+        myview->GetPosXY(pos_x,pos_y,view_x,view_y);
+        mppixsteer->setPos(view_x  ,view_y );
+        mfsteerposx = view_x;
+        mfsteerposy = view_y;
+
+        if(beishu == 0.0)return;
+        mpitemstate->setScale(1.0/beishu);
+        mpitemstate->setPos(view_x + fViewSize /beishu,view_y);
+        mpitemcmd->setScale(1.0/beishu);
+        mpitemcmd->setPos(view_x + fViewSize /beishu,view_y + (mnFontSize*1.5)/beishu);
+    }
+
+    double s,t;
+    double fviewhalf = mfsteerviewsize * 0.5 /beishu;
+    double frot = fwheelangle * M_PI/180.0;
+    x0 = fviewhalf;
+    y0 = fviewhalf *(-1.0);
+    s = x0 * cos(frot) - y0 * sin(frot);
+    t = x0 * sin(frot) + y0 * cos(frot);
+
+    mppixsteer->setPos(mfsteerposx + x0 - s, mfsteerposy - y0 + t);
 }
 
 void MainWindow::onSetDstPos(double x, double y)
@@ -1111,7 +1206,14 @@ void MainWindow::UpdateDecition(const char * strdata,const unsigned int nSize,co
     //ServiceCarStatus.mfAcc = xdecition.accelerator();   //
 
     if(mbSimulate)
+    {
         mpsimmodel->SetCMD(xdecition.accelerator(),xdecition.torque(),xdecition.brake(),xdecition.wheelangle());
+
+        mmutexdecition.lock();
+        mdecition.CopyFrom(xdecition);
+        mnlastdecitiontime = std::chrono::system_clock::now().time_since_epoch().count();
+        mmutexdecition.unlock();
+    }
 }
 
 void MainWindow::UpdateXodrObj(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -1300,3 +1402,14 @@ void MainWindow::on_actionTrack_triggered()
     }
 }
 
+void MainWindow::LoadXML()
+{
+    iv::xmlparam::Xmlparam xp("simple_planning_simulator.xml");
+
+    mfSensorRange = xp.GetParam("SensorRange",50.0);
+    mfWHEELSPEED = xp.GetParam("WheelMaxSpeed",300.0);
+    mfwheelbase = xp.GetParam("WheelBase",2.9);
+    mfwheelratio = xp.GetParam("WheelRatio",13.6);
+    mfVehWeight = xp.GetParam("VehicleWeight",1800.0);
+}
+

src/tool/simple_planning_simulator/mainwindow.h

@@ -19,6 +19,8 @@
 
 #include "simmodel_shenlan.h"
 
+#include "xmlparam.h"
+
 
 #include "decition.pb.h"
 
@@ -220,6 +222,16 @@ private:
 
     bool mbTrack = false;
 
+    double mfWheelAngle = 0.0;
+    double mfScale = 1.0;
+    double mbeishu = 1.0;
+
+    double mfsteerposx,mfsteerposy;
+
+    iv::brain::decition mdecition;
+    int64_t mnlastdecitiontime = 0;
+    std::mutex mmutexdecition;
+
 
 private:
     int mnFontHeight;
@@ -258,6 +270,12 @@ private:
 
     std::vector<QGraphicsEllipseItem *> mvectormappointitem;
 
+    double mfSensorRange = 50.0;
+    double mfWHEELSPEED = 300;
+    double mfwheelbase = 2.9;
+    double mfwheelratio = 13.6;
+    double mfVehWeight = 1800.0;
+
 
 
 public:
@@ -284,5 +302,7 @@ private:
     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);
 
+    void LoadXML();
+
 };
 #endif // MAINWINDOW_H

src/tool/simple_planning_simulator/sim.qrc

@@ -5,5 +5,6 @@
         <file>sp.png</file>
         <file>simicon.png</file>
         <file>steer.png</file>
+        <file>TrafficLight.png</file>
     </qresource>
 </RCC>

src/tool/simple_planning_simulator/simmodel.cpp

@@ -342,4 +342,46 @@ void simmodel::DelSimobj(unsigned int index)
     mmutexsimobj.unlock();
 }
 
+void simmodel::SetSensorRange(double fSensorRange)
+{
+    mfSensorRange = fSensorRange;
+}
+void simmodel::SetWheelMaxSpeed(double fWheelMaxSpeed)
+{
+    mfWHEELSPEED = fWheelMaxSpeed;
+}
+void simmodel::SetWheelBase(double fWheelBase)
+{
+    mfwheelbase = fWheelBase;
+}
+void simmodel::SetWheelRatio(double fWheelRatio)
+{
+    mfwheelratio = fWheelRatio;
+}
+void simmodel::SetVehWeight(double fVehWeight)
+{
+    mfVehWeight = fVehWeight;
+}
+
+double simmodel::GetSensorRange()
+{
+    return mfSensorRange;
+}
+double simmodel::GetWheelMaxSpeed()
+{
+    return mfWHEELSPEED;
+}
+double simmodel::GetWheelBase()
+{
+    return mfwheelbase;
+}
+double simmodel::GetWheelRatio()
+{
+    return mfwheelratio;
+}
+double simmodel::GetVehWeight()
+{
+    return mfVehWeight;
+}
+
 

src/tool/simple_planning_simulator/simmodel.h

@@ -63,6 +63,8 @@ protected:
     double mfWHEELSPEED = 300;
     double mfwheelbase = 2.9;
     double mfwheelratio = 13.6;
+    double mfVehWeight = 1800.0;
+    double mfviewx,mfviewy;
 
 public:
     double GetX();
@@ -76,6 +78,20 @@ public:
 
     virtual void CalcModel() = 0;
 
+
+public:
+    void SetSensorRange(double fSensorRange);
+    void SetWheelMaxSpeed(double fWheelMaxSpeed);
+    void SetWheelBase(double fWheelBase);
+    void SetWheelRatio(double fWheelRatio);
+    void SetVehWeight(double fVehWeight);
+
+    double GetSensorRange();
+    double GetWheelMaxSpeed();
+    double GetWheelBase();
+    double GetWheelRatio();
+    double GetVehWeight();
+
 protected:
     double mfcmd_acc;
     double mfcmd_torque;
@@ -115,6 +131,8 @@ private:
     std::mutex mmutexsimobj;
 
 
+
+
 };
 
 #endif // SIMMODEL_H

src/tool/simple_planning_simulator/simmodel_shenlan.cpp

@@ -65,7 +65,7 @@ void simmodel_shenlan::CalcModel()
 
         double facc;
 
-        double fVehWeight = 1800;
+        double fVehWeight = mfVehWeight;
 //        double fg = 9.8;
         double fRollForce = 50;
         const double fRatio = 2.5;

src/tool/simple_planning_simulator/simple_planning_simulator.xml

@@ -0,0 +1,9 @@
+<xml>	
+	<node name="simple_planning_simulator">
+		<param name="SensorRange" value="50.0" />
+		<param name="WheelMaxSpeed" value="300" />
+		<param name="WheelBase" value="2.9" />
+		<param name="WheelRatio" value="13.6" />
+		<param name="VehicleWeight" value="1800.0" />
+	</node>
+</xml>