add simple_planning_simulator_rd. add real dynamics.

src/tool/simple_planning_simulator/simmodel.cpp

@@ -17,7 +17,15 @@ simmodel::simmodel()
 
     mpa = iv::modulecomm::RegisterSend("hcp2_gpsimu",100000,1);
     mpatrack = iv::modulecomm::RegisterSend("lidar_track",1000000,1);
+
+#ifndef REAL_DYNAMICS
     mpachassis = iv::modulecomm::RegisterSend("chassis",10000,1);
+#endif
+
+#ifdef REAL_DYNAMICS
+    ModuleFun funchassis =std::bind(&simmodel::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mparealchassis = iv::modulecomm::RegisterRecvPlus("chassis",funchassis);
+#endif
 
     mpthread = new std::thread(&simmodel::threadupdate,this);
     mpthreadstate = new std::thread(&simmodel::threadstate,this);
@@ -31,6 +39,29 @@ simmodel::~simmodel()
     mpthreadstate->join();
 }
 
+void simmodel::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    iv::chassis xchassis;
+    static int ncount = 0;
+    if(!xchassis.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
+        return;
+    }
+
+    if(xchassis.has_vel())
+    {
+        mfRealVel = xchassis.vel();
+    }
+
+    if(xchassis.has_angle_feedback())
+    {
+        mfRealSteer = xchassis.angle_feedback();
+    }
+
+
+}
+
 void simmodel::SetCMD(double facc,double ftorque,double fbrake,double fwheelangle)
 {
     mfcmd_acc = facc;
@@ -299,6 +330,7 @@ void simmodel::threadstate()
         //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
         iv::modulecomm::ModuleSendMsg(mpatrack,out.data(),out.length());
 
+#ifndef REAL_DYNAMICS
         iv::chassis xchassis;
         xchassis.set_angle_feedback(mwheelsteer * mfwheelratio * 180.0/M_PI);
         xchassis.set_vel(mvel * 3.6);
@@ -318,6 +350,7 @@ void simmodel::threadstate()
         {
             std::cout<<" chassis serializetoarray fail."<<std::endl;
         }
+#endif
 
         std::this_thread::sleep_for(std::chrono::milliseconds(20));
     }

src/tool/simple_planning_simulator/simmodel.h

@@ -66,6 +66,17 @@ protected:
     double mfVehWeight = 1800.0;
     double mfviewx,mfviewy;
 
+#ifdef REAL_DYNAMICS
+    double mfRealVel = 0;
+    double mfRealSteer = 0;
+    void * mparealchassis;
+
+#endif
+
+
+private:
+    void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname);
+
 public:
     double GetX();
     double GetY();

src/tool/simple_planning_simulator/simmodel_shenlan.cpp

@@ -105,7 +105,11 @@ void simmodel_shenlan::CalcModel()
             mvel = 0;
             macc = 0;
         }
+#ifndef REAL_DYNAMICS
         mvel = mvel + facc * fdifftime;
+#else
+        mvel = mfRealVel/3.6;
+#endif
 
 //        std::cout<<" acc: "<<macc<<std::endl;
 
@@ -124,7 +128,13 @@ void simmodel_shenlan::CalcModel()
         }
 
         fwheelsteer = fwheelsteer *(M_PI/180.0) /mfwheelratio;
+#ifndef REAL_DYNAMICS
         mwheelsteer = fwheelsteer;
+#else
+        mwheelsteer = mfRealSteer /( mfwheelratio * 180.0/M_PI);
+#endif
+
+
 
 
 

src/tool/simple_planning_simulator/simple_planning_simulator_rd.pro

@@ -0,0 +1,127 @@
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+CONFIG += c++17
+
+QMAKE_LFLAGS += -no-pie
+
+
+#DEFINES += SHOW_ONLY
+
+if(contains(DEFINES,SHOW_ONLY)){
+TARGET = simple_planning_simulator11
+}
+
+DEFINES += REAL_DYNAMICS
+
+
+
+# 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/math/gnss_coordinate_convert.cpp \
+    ../../common/common/xodr/xodrfunc/roadsample.cpp \
+    ../../include/msgtype/chassis.pb.cc \
+    ../../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 \
+    ../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 \
+    dialogcustomobj.cpp \
+    main.cpp \
+    mainwindow.cpp \
+    simmodel.cpp \
+    simmodel_shenlan.cpp
+
+HEADERS += \
+    ../../common/common/math/gnss_coordinate_convert.h \
+    ../../common/common/xodr/xodrfunc/roadsample.h \
+    ../../include/msgtype/chassis.pb.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 \
+    ../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 \
+    dialogcustomobj.h \
+    mainwindow.h \
+    simmodel.h \
+    simmodel_shenlan.h
+
+FORMS += \
+    dialogcustomobj.ui \
+    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(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+
+!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
+
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
+
+
+INCLUDEPATH += $$PWD/../../common/common/license_local
+include($$PWD/../../common/common/license_local/adclicense.pri)
+
+#unix:LIBS += -lboost_thread -lboost_system -lboost_serialization -lprotobuf
+
+RESOURCES += \
+    sim.qrc