diff --git a/CMakeLists.txt b/CMakeLists.txt index 2267c5a..94d3613 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,8 @@ set(SOURCES src/gltrafficsign.cpp src/qrc_resources.cpp src/osiparser.cpp + src/glfieldofview.cpp + src/fmureceiver.cpp ) set(HEADERS @@ -55,9 +57,15 @@ set(HEADERS include/glpoint.h include/gltrafficsign.h include/osiparser.h + include/glfieldofview.h + include/fmureceiver.h +) + + +include_directories( + ${CMAKE_CURRENT_LIST_DIR}/include ) -include_directories(include) add_executable(${PROJECT_NAME} ${HEADERS} @@ -72,7 +80,9 @@ target_link_libraries(${PROJECT_NAME} Qt5::OpenGL Qt5::Network - open_simulation_interface_pic + open_simulation_interface + fmilib_shared protobuf zmq ) + diff --git a/Dockerfile b/Dockerfile index 5fc2e48..607dcc7 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,7 +1,10 @@ FROM ubuntu:latest RUN mkdir -p /project -RUN apt-get update && apt-get install -y git vim unzip zip cmake gcc g++ qtbase5-dev libprotobuf-dev protobuf-compiler libzmq3-dev +RUN apt-get update && apt-get install -y git vim unzip zip cmake gcc g++ qtbase5-dev libprotobuf-dev protobuf-compiler libzmq3-dev wget +RUN mkdir -p /build +WORKDIR /build +RUN wget http://www.jmodelica.org/downloads/FMIL/FMILibrary-2.0.3-src.zip && unzip FMILibrary-2.0.3-src.zip && mkdir build-fmil && cd build-fmil && cmake -DFMILIB_INSTALL_PREFIX=/usr/local ../FMILibrary-2.0.3 && make install VOLUME [ "/project"] WORKDIR /project diff --git a/include/appconfig.h b/include/appconfig.h index 8b3765e..b9e8a66 100644 --- a/include/appconfig.h +++ b/include/appconfig.h @@ -21,20 +21,39 @@ class AppConfig QString ch1IPAddress_; QString ch1PortNum_; DataType ch1DataType_; + bool ch1FMURxCheck_; + QString ch1LoadFMURx_; QString ch1LoadFile_; DataType ch1PlaybackDataType_; int ch1DeltaDelay_; bool ch1EnableSendOut_; QString ch1SendOutPortNum_; + bool ch1FMUTxCheck_; + QString ch1LoadFMUTx_; + bool ch1ShowFOV_; + float ch1MinRadius_; + float ch1MaxRadius_; + float ch1AzimuthPos_; + float ch1AzimuthNeg_; + QString ch2IPAddress_; QString ch2PortNum_; DataType ch2DataType_; + bool ch2FMURxCheck_; + QString ch2LoadFMURx_; QString ch2LoadFile_; DataType ch2PlaybackDataType_; int ch2DeltaDelay_; bool ch2EnableSendOut_; QString ch2SendOutPortNum_; + bool ch2FMUTxCheck_; + QString ch2LoadFMUTx_; + bool ch2ShowFOV_; + float ch2MinRadius_; + float ch2MaxRadius_; + float ch2AzimuthPos_; + float ch2AzimuthNeg_; bool combineChannel_; bool showGrid_; diff --git a/include/fmureceiver.h b/include/fmureceiver.h new file mode 100644 index 0000000..4a4aa29 --- /dev/null +++ b/include/fmureceiver.h @@ -0,0 +1,110 @@ +/// +/// @file +/// @copyright Copyright (C) 2017, Bayerische Motoren Werke Aktiengesellschaft (BMW AG) +/// +/// @brief +/// + + + +#pragma once + +#include + +#include + +#include "osi_sensordata.pb.h" +#include "imessagesource.h" +#include "types.h" + + +extern "C" { +#include +#include +} + +/* The following names come from FMU modelDescription.in.xml */ +#define FMI_SENDER_NAME "sender" +#define FMI_RECEIVER_NAME "receiver" +#define FMI_ADDRESS_NAME "address" +#define FMI_PORT_NAME "port" + +#define FMI_DATA_OUT_BASELO_NAME "OSMPSensorDataOut.base.lo" +#define FMI_DATA_OUT_BASEHI_NAME "OSMPSensorDataOut.base.hi" +#define FMI_DATA_OUT_SIZE_NAME "OSMPSensorDataOut.size" + +/* local index defines */ +#define FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX 0 // correspond to FMI_DATA_OUT_BASELO_NAME +#define FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX 1 // correspond to FMI_DATA_OUT_BASEHI_NAME +#define FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX 2 // correspond to FMI_DATA_OUT_SIZE_NAME +#define FMI_INTEGER_LAST_OUT_IDX FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX +#define FMI_INTEGER_OUT_VARS (FMI_INTEGER_LAST_OUT_IDX + 1) + +class FMUReceiver: public QObject, public IMessageSource +{ + Q_OBJECT + + public: + FMUReceiver(); + + signals: + void Connected(DataType dataType); + void Disconnected(const QString& message = ""); + void MessageReceived(const osi::SensorData& sensorData, + const DataType datatype); + + public slots: + void DisconnectRequested(); + void ConnectRequested(const QString& ipAddress, + const QString& port, + const QString& fmuPath, + DataType dataType); + + private: + + void ReceiveLoop(); + + + bool isRunning_; + bool isThreadTerminated_; + DataType currentDataType_; + + + // FMU interface + fmi2_import_t* fmu_; + fmi2_callback_functions_t callBackFunctions_; + jm_callbacks callbacks_; + fmi_import_context_t* context_; + + jm_status_enu_t jmStatus_; + fmi2_status_t fmiStatus_; + + fmi2_real_t tStart_; + fmi2_real_t tEnd_; + fmi2_real_t tCurrent_; + fmi2_real_t hStep_; + + std::string ip_; + std::string port_; + std::string FMUPath_; + std::string tmpPath_; + enum class LogLevel + { + Warn, + Debug + }; + LogLevel logLevel_; + std::string currentBuffer_; + fmi2_value_reference_t vr_[FMI_INTEGER_OUT_VARS]; + + // initialize fmu wrapper specific logger, create fmi import context and check fmi version + bool initializeFMUWrapper(); + // import fmu binary file + bool importFMU(); + // setup and initialize FMU + bool initializeFMU(); + // protobuf accessors + bool get_fmi_sensor_data_in(osi::SensorData& data); +}; + + diff --git a/include/glfieldofview.h b/include/glfieldofview.h new file mode 100644 index 0000000..0f059c2 --- /dev/null +++ b/include/glfieldofview.h @@ -0,0 +1,23 @@ + + + + +#pragma once +#include "globject.h" + +class GLFieldOfView : public GLObject +{ + public: + GLFieldOfView(QOpenGLFunctions_4_3_Core* functions, + const float minRadius, + const float maxRadius, + const float azimuthPosAngle, + const float azimuthNegAngle); + + void UpdateParameter(const float minRadius, + const float maxRadius, + const float azimuthPosAngle, + const float azimuthNegAngle); +}; + + diff --git a/include/glvehicle.h b/include/glvehicle.h index a7f437d..cf3ce03 100644 --- a/include/glvehicle.h +++ b/include/glvehicle.h @@ -12,4 +12,11 @@ class GLVehicle : public GLObject { public: GLVehicle(QOpenGLFunctions_4_3_Core* functions, QString id, float xExtend, float zExtend); + + GLVehicle(QOpenGLFunctions_4_3_Core* functions, + QString id, + QVector3D v0, + QVector3D v1, + QVector3D v2, + QVector3D v3); }; diff --git a/include/glwidget.h b/include/glwidget.h index d8dabbd..f4de771 100644 --- a/include/glwidget.h +++ b/include/glwidget.h @@ -14,6 +14,8 @@ #include "lane.h" #include "imessagesource.h" +#include "glfieldofview.h" + #include #include #include @@ -35,6 +37,13 @@ class GLWidget : public QOpenGLWidget, protected QOpenGLFunctions_4_3_Core void UpdateIMessageSource(IMessageSource* msgSource); + void UpdateFOVPaint(const bool showFOV); + + void UpdateFOVParam(const float minRadius, + const float maxRadius, + const float azimuthPosAngle, + const float azimuthNegAngle); + signals: void DisplayObjectInformation(GLObject* object); void SetTrackingEnabled(bool enable); @@ -87,4 +96,7 @@ class GLWidget : public QOpenGLWidget, protected QOpenGLFunctions_4_3_Core QOpenGLShaderProgram shaderProgram_; QMap& treeNodes_; + bool showFOV_; + GLFieldOfView* objFOV_; + }; diff --git a/include/mainwindow.h b/include/mainwindow.h index 7cafb34..2a4f0eb 100644 --- a/include/mainwindow.h +++ b/include/mainwindow.h @@ -18,6 +18,7 @@ class GLWidget; class TCPReceiver; +class FMUReceiver; class AppConfig; class OsiParser; class OsiReader; @@ -35,6 +36,8 @@ class MainWindow : public QMainWindow explicit MainWindow(QWidget* parent = nullptr); ~MainWindow(); + void LocalUpdate(); + signals: void UpdateGrid(); void UpdateLane(); @@ -47,11 +50,23 @@ class MainWindow : public QMainWindow const QString& port, const DataType dataType); + void FMUConnectRequested(const QString& ipAddress, + const QString& port, + const QString& fmuPath, + const DataType dataType); + + void FMUConnectRequested2(const QString& ipAddress, + const QString& port, + const QString& fmuPath, + const DataType dataType); + void StartPlaybackRequested(const QString& fileName, - const DataType dataType); + const DataType dataType, + const QString& fmuPath); void StartPlaybackRequested2(const QString& fileName, - const DataType dataType); + const DataType dataType, + const QString& fmuPath); public slots: void EnableExport(bool enable); @@ -68,7 +83,9 @@ class MainWindow : public QMainWindow void UpdateSliderRange(int sliderRange); void UpdateSliderRange2(int sliderRange); void UpdateSliderValue(int sliderValue); + void UpdateSliderTime(int sliderValue); void UpdateSliderValue2(int sliderValue); + void UpdateSliderTime2(int sliderValue); void DisplayObjectInformation(GLObject* object); void DisplayObjectInformation2(GLObject* object); @@ -91,9 +108,29 @@ class MainWindow : public QMainWindow void RBPlayback(); void RBPlayback2(); + void CBDataTypeCon(int index); + void CBDataTypePlay(int index); + void CBDataTypeCon2(int index); + void CBDataTypePlay2(int index); + void ToggleShowFOV(); + void ToggleShowFOV2(); + + void CheckBoxFMURx(); + void CheckBoxFMURx2(); + void LoadFMURxEdited(const QString& text); + void LoadFMURxBrowse(); + void LoadFMURxEdited2(const QString& text); + void LoadFMURxBrowse2(); + + void CheckBoxFMUTx(); + void CheckBoxFMUTx2(); + void LoadFMUTxEdited(const QString& text); + void LoadFMUTxBrowse(); + void LoadFMUTxEdited2(const QString& text); + void LoadFMUTxBrowse2(); + void LoadFileEdited(const QString& text); void LoadFileBrowse(); - void LoadFileEdited2(const QString& text); void LoadFileBrowse2(); @@ -112,6 +149,9 @@ class MainWindow : public QMainWindow void CombineChannels(); + void ShowFOV(); + void ShowFOV2(); + private: @@ -184,6 +224,9 @@ class MainWindow : public QMainWindow void ShowErrorMessage(const QString& errMsg); + void EnableShowFOV(const bool enable); + void EnableShowFOV2(const bool enable); + // Configurations AppConfig config_; @@ -211,12 +254,14 @@ class MainWindow : public QMainWindow Ui::MainWindow *ui_; GLWidget* glWidget_; - TCPReceiver* receiver_; + TCPReceiver* tcpReceiver_; + FMUReceiver* fmuReceiver_; OsiReader* reader_; OsiParser* osiparser_; GLWidget* glWidget2_; - TCPReceiver* receiver2_; + TCPReceiver* tcpReceiver2_; + FMUReceiver* fmuReceiver2_; OsiReader* reader2_; OsiParser* osiparser2_; diff --git a/include/osireader.h b/include/osireader.h index 5792c0f..aaf84bd 100644 --- a/include/osireader.h +++ b/include/osireader.h @@ -8,8 +8,8 @@ #pragma once -#include -#include +#include "osi_version.pb.h" +#include "osi_sensordata.pb.h" #include @@ -24,6 +24,29 @@ #include "types.h" +extern "C" { +#include +#include +} + + +#define FMI_SENDER_NAME "sender" +#define FMI_RECEIVER_NAME "receiver" +#define FMI_ADDRESS_NAME "address" +#define FMI_PORT_NAME "port" + +#define FMI_DATA_IN_BASELO_NAME "OSMPSensorViewIn.base.lo" +#define FMI_DATA_IN_BASEHI_NAME "OSMPSensorViewIn.base.hi" +#define FMI_DATA_IN_SIZE_NAME "OSMPSensorViewIn.size" + +/* local index defines */ +#define FMI_INTEGER_SENSORDATA_IN_BASELO_IDX 0 // correspond to FMI_DATA_IN_BASELO_NAME +#define FMI_INTEGER_SENSORDATA_IN_BASEHI_IDX 1 // correspond to FMI_DATA_IN_BASEHI_NAME +#define FMI_INTEGER_SENSORDATA_IN_SIZE_IDX 2 // correspond to FMI_DATA_IN_SIZE_NAME +#define FMI_INTEGER_LAST_IN_IDX FMI_INTEGER_SENSORDATA_IN_SIZE_IDX +#define FMI_INTEGER_IN_VARS (FMI_INTEGER_LAST_IN_IDX + 1) + + class OsiReader: public QObject, public IMessageSource { Q_OBJECT @@ -31,11 +54,13 @@ class OsiReader: public QObject, public IMessageSource public: OsiReader(int* deltaDelay, const bool& enableSendOut, - const std::string& zmqPubPortNum); + const std::string& pubPortNum, + const bool &enalbeFMU, + const std::string &fmuPath = ""); QString SetupConnection(bool enable); - void SetSendOutPortNum(const std::string& port) { zmqPubPortNumber_ = port; } + void SetSendOutPortNum(const std::string& port) { pubPortNumber_ = port; } signals: void Connected(DataType dataType); @@ -46,7 +71,9 @@ class OsiReader: public QObject, public IMessageSource const DataType datatype); public slots: - void StartReadFile(const QString& osiFileName, const DataType dataType); + void StartReadFile(const QString& osiFileName, + const DataType dataType, + const QString& fmuPath); void StopReadFile(); void SliderValueChanged(int newValue); @@ -59,8 +86,12 @@ class OsiReader: public QObject, public IMessageSource void SaveHeader(); void SendMessageLoop(); + void SendOutMessage(const std::string& message); - void ZMQSendOutMessage(const std::string& message); + QString SetZMQConnection(); + QString FreeZMQConnection(); + QString SetFMUConnection(); + void FreeFMUConnection(); @@ -70,6 +101,7 @@ class OsiReader: public QObject, public IMessageSource QString osiFileName_; QString osiHeaderName_; + uint64_t firstTimeStamp_; // std::map