#pragma once #include #include #include "QGCMapPolygon.h" #include "QmlObjectListModel.h" #include "Geometry/WimaArea.h" #include "Geometry/WimaMeasurementArea.h" #include "Geometry/WimaServiceArea.h" #include "Geometry/WimaCorridor.h" #include "WimaDataContainer.h" #include "Geometry/WimaMeasurementAreaData.h" #include "Geometry/WimaCorridorData.h" #include "Geometry/WimaServiceAreaData.h" #include "PlanMasterController.h" #include "MissionController.h" #include "SurveyComplexItem.h" #include "SimpleMissionItem.h" #include "MissionSettingsItem.h" #include "JsonHelper.h" #include "QGCApplication.h" #include "SettingsFact.h" #include "WimaSettings.h" #include "SettingsManager.h" #include "snake.h" #include "Snake/SnakeWorker.h" #include "Snake/SnakeTiles.h" #include "Snake/SnakeTilesLocal.h" #include "Geometry/GeoPoint3D.h" #include "Snake/QNemoProgress.h" #include "ros_bridge/include/ROSBridge.h" #include "WaypointManager/DefaultManager.h" #define CHECK_BATTERY_INTERVAL 1000 // ms #define SMART_RTL_MAX_ATTEMPTS 3 // times #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms #define EVENT_TIMER_INTERVAL 50 // ms #define SNAKE_EVENT_LOOP_INTERVAL 5000 // ms using namespace snake; typedef std::unique_ptr JsonDocUPtr; class WimaController : public QObject { Q_OBJECT enum FileType {WimaFile, PlanFile}; typedef QScopedPointer ROSBridgePtr; public: enum SnakeConnectionStatus {Connected = 1, NotConnected = 0}; WimaController(QObject *parent = nullptr); Q_PROPERTY(PlanMasterController* masterController READ masterController WRITE setMasterController NOTIFY masterControllerChanged ) Q_PROPERTY(MissionController* missionController READ missionController WRITE setMissionController NOTIFY missionControllerChanged ) Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged ) // Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged) // Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT) // Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) // Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) Q_PROPERTY(WimaDataContainer* dataContainer READ dataContainer WRITE setDataContainer NOTIFY dataContainerChanged ) Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged ) Q_PROPERTY(QmlObjectListModel* currentMissionItems READ currentMissionItems NOTIFY currentMissionItemsChanged ) Q_PROPERTY(QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged ) Q_PROPERTY(QVariantList currentWaypointPath READ currentWaypointPath NOTIFY currentWaypointPathChanged ) Q_PROPERTY(Fact* enableWimaController READ enableWimaController CONSTANT) Q_PROPERTY(Fact* overlapWaypoints READ overlapWaypoints CONSTANT ) Q_PROPERTY(Fact* maxWaypointsPerPhase READ maxWaypointsPerPhase CONSTANT ) Q_PROPERTY(Fact* startWaypointIndex READ startWaypointIndex CONSTANT ) Q_PROPERTY(Fact* showAllMissionItems READ showAllMissionItems CONSTANT ) Q_PROPERTY(Fact* showCurrentMissionItems READ showCurrentMissionItems CONSTANT ) Q_PROPERTY(Fact* flightSpeed READ flightSpeed CONSTANT ) Q_PROPERTY(Fact* altitude READ altitude CONSTANT ) Q_PROPERTY(Fact* arrivalReturnSpeed READ arrivalReturnSpeed CONSTANT ) Q_PROPERTY(bool uploadOverrideRequired READ uploadOverrideRequired WRITE setUploadOverrideRequired NOTIFY uploadOverrideRequiredChanged ) Q_PROPERTY(double phaseDistance READ phaseDistance NOTIFY phaseDistanceChanged ) Q_PROPERTY(double phaseDuration READ phaseDuration NOTIFY phaseDurationChanged ) Q_PROPERTY(bool vehicleHasLowBattery READ vehicleHasLowBattery NOTIFY vehicleHasLowBatteryChanged ) // Snake Q_PROPERTY(Fact* enableSnake READ enableSnake CONSTANT ) Q_PROPERTY(long snakeConnectionStatus READ snakeConnectionStatus NOTIFY snakeConnectionStatusChanged ) Q_PROPERTY(bool snakeCalcInProgress READ snakeCalcInProgress NOTIFY snakeCalcInProgressChanged ) Q_PROPERTY(Fact* snakeTileWidth READ snakeTileWidth CONSTANT ) Q_PROPERTY(Fact* snakeTileHeight READ snakeTileHeight CONSTANT ) Q_PROPERTY(Fact* snakeMinTileArea READ snakeMinTileArea CONSTANT ) Q_PROPERTY(Fact* snakeLineDistance READ snakeLineDistance CONSTANT ) Q_PROPERTY(Fact* snakeMinTransectLength READ snakeMinTransectLength CONSTANT ) Q_PROPERTY(QmlObjectListModel* snakeTiles READ snakeTiles NOTIFY snakeTilesChanged ) Q_PROPERTY(QVariantList snakeTileCenterPoints READ snakeTileCenterPoints NOTIFY snakeTileCenterPointsChanged ) Q_PROPERTY(QVector nemoProgress READ nemoProgress NOTIFY nemoProgressChanged ) // Property accessors PlanMasterController* masterController (void); MissionController* missionController (void); QmlObjectListModel* visualItems (void); // QString currentFile (void) const { return _currentFile; } // QStringList loadNameFilters (void) const; // QStringList saveNameFilters (void) const; // QString fileExtension (void) const { return wimaFileExtension; } QGCMapPolygon joinedArea (void) const; WimaDataContainer* dataContainer (void); QmlObjectListModel* missionItems (void); QmlObjectListModel* currentMissionItems (void); QVariantList waypointPath (void); QVariantList currentWaypointPath (void); Fact* enableWimaController (void); Fact* overlapWaypoints (void); Fact* maxWaypointsPerPhase (void); Fact* startWaypointIndex (void); Fact* showAllMissionItems (void); Fact* showCurrentMissionItems(void); Fact* flightSpeed (void); Fact* arrivalReturnSpeed (void); Fact* altitude (void); Fact* enableSnake (void) { return &_enableSnake; } Fact* snakeTileWidth (void) { return &_snakeTileWidth;} Fact* snakeTileHeight (void) { return &_snakeTileHeight;} Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;} Fact* snakeLineDistance (void) { return &_snakeLineDistance;} Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;} QmlObjectListModel* snakeTiles (void) { return _snakeTiles.QmlObjectListModel();} QVariantList snakeTileCenterPoints (void) { return _snakeTileCenterPoints;} QVector nemoProgress (void) { return _nemoProgress.progress();} bool uploadOverrideRequired (void) const; double phaseDistance (void) const; double phaseDuration (void) const; bool vehicleHasLowBattery (void) const; long snakeConnectionStatus (void) const; bool snakeCalcInProgress (void) const; // Property setters void setMasterController (PlanMasterController* masterController); void setMissionController (MissionController* missionController); void setDataContainer (WimaDataContainer* container); void setUploadOverrideRequired (bool overrideRequired); // Member Methodes Q_INVOKABLE void nextPhase(); Q_INVOKABLE void previousPhase(); Q_INVOKABLE void resetPhase(); Q_INVOKABLE bool uploadToVehicle(); Q_INVOKABLE bool forceUploadToVehicle(); Q_INVOKABLE void removeFromVehicle(); Q_INVOKABLE bool checkSmartRTLPreCondition(); Q_INVOKABLE bool calcReturnPath(); Q_INVOKABLE void executeSmartRTL(); Q_INVOKABLE void initSmartRTL(); Q_INVOKABLE void removeVehicleTrajectoryHistory(); // Q_INVOKABLE void saveToCurrent (); // Q_INVOKABLE void saveToFile (const QString& filename); // Q_INVOKABLE bool loadFromCurrent(); // Q_INVOKABLE bool loadFromFile (const QString& filename); // static Members // static const char* wimaFileExtension; static const char* areaItemsName; static const char* missionItemsName; static const char* settingsGroup; static const char* endWaypointIndexName; static const char* enableWimaControllerName; static const char* overlapWaypointsName; static const char* maxWaypointsPerPhaseName; static const char* startWaypointIndexName; static const char* showAllMissionItemsName; static const char* showCurrentMissionItemsName; static const char* flightSpeedName; static const char* arrivalReturnSpeedName; static const char* altitudeName; static const char* snakeTileWidthName; static const char* snakeTileHeightName; static const char* snakeMinTileAreaName; static const char* snakeLineDistanceName; static const char* snakeMinTransectLengthName; // Member Methodes QJsonDocument saveToJson(FileType fileType); bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path); signals: void masterControllerChanged (void); void missionControllerChanged (void); void visualItemsChanged (void); // void currentFileChanged (); void dataContainerChanged (); void readyForSaveSendChanged (bool ready); void missionItemsChanged (void); void currentMissionItemsChanged (void); void waypointPathChanged (void); void currentWaypointPathChanged (void); void uploadOverrideRequiredChanged (void); void phaseDistanceChanged (void); void phaseDurationChanged (void); void vehicleHasLowBatteryChanged (void); void returnBatteryLowConfirmRequired (void); void returnUserRequestConfirmRequired (void); void snakeConnectionStatusChanged (void); void snakeCalcInProgressChanged (void); void snakeTilesChanged (void); void snakeTileCenterPointsChanged (void); void nemoProgressChanged (void); private: enum SRTL_Reason {BatteryLow, UserRequest}; private slots: bool _fetchContainerData(); bool _calcNextPhase(void); //void _updateWaypointPath (void); //void _updateCurrentPath (void); //void _updateNextWaypoint (void); void _recalcCurrentPhase (void); //bool _setTakeoffLandPosition (void); void _updateOverlap (void); void _updateMaxWaypoints (void); void _updateflightSpeed (void); void _updateArrivalReturnSpeed (void); void _updateAltitude (void); void _checkBatteryLevel (void); void _eventTimerHandler (void); void _smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL void _enableDisableLowBatteryHandling (QVariant enable); void _initSmartRTL (); void _executeSmartRTL (); void _setSnakeConnectionStatus (SnakeConnectionStatus status); void _setSnakeCalcInProgress (bool inProgress); bool _verifyScenarioDefined (void); bool _verifyScenarioDefinedWithErrorMessage (void); void _snakeStoreWorkerResults (); void _startStopRosBridge (); void _initStartSnakeWorker (); private: void _setPhaseDistance(double distance); void _setPhaseDuration(double duration); bool _checkSmartRTLPreCondition(QString &errorString); // should be called from gui, befor calcReturnPath() bool _calcReturnPath(QString &errorSring); // Calculates return path (destination: service area center) for a flying vehicle void _setVehicleHasLowBattery(bool batteryLow); void _loadCurrentMissionItemsFromBuffer(); void _saveCurrentMissionItemsToBuffer(); void _progressFromJson(JsonDocUPtr pDoc, QNemoProgress &progress); PlanMasterController *_masterController; MissionController *_missionController; // QString _currentFile; // file for saveing WimaDataContainer *_container; // container for data exchange with WimaController QmlObjectListModel _areas; // contains all visible areas WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor WimaMeasurementAreaData _measurementArea; // measurement area WimaServiceAreaData _serviceArea; // area for supplying WimaCorridorData _corridor; // corridor connecting opArea and serArea bool _localPlanDataValid; WaypointManager::AreaInterface _areaInterface; WaypointManager::Settings _managerSettings; WaypointManager::DefaultManager _defaultManager; WaypointManager::DefaultManager _snakeManager; WaypointManager::ManagerBase &_currentManager; // QmlObjectListModel _missionItems; // all mission itmes (Mission Items) generaded by wimaPlaner, displayed in flightView // QmlObjectListModel _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems, // // _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage // QmlObjectListModel _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked // QVector _waypoints; // path connecting the items in _missionItems // QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems // QGeoCoordinate _takeoffLandPostion; QMap _metaDataMap; SettingsFact _enableWimaController; // enables or disables the wimaControler SettingsFact _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints per phase SettingsFact _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item // defining the first element of the next phase SettingsFact _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not. SettingsFact _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not. SettingsFact _flightSpeed; // mission flight speed SettingsFact _arrivalReturnSpeed; // arrival and return path speed SettingsFact _altitude; // mission altitude SettingsFact _enableSnake; // Enable Snake (see snake.h) // int _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element // // (which is not part of the return path) of _currentMissionItem // int _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element // // (which is not part of the arrival path) of _currentMissionItem bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area. // The user can override the upload lock with a slider, this will reset this variable to false. double _measurementPathLength; // the lenght of the phase in meters // double _arrivalPathLength; // the length of the arrival and return path in meters // double _returnPathLength; // the length of the arrival and return path in meters // double _phaseDistance; // the lenth in meters of the current phase // double _phaseDuration; // the phase duration in seconds // double _phaseDistanceBuffer; // buffer for storing _phaseDistance when doing smart RTL // double _phaseDurationBuffer; // buffer for storing _phaseDuration when doing smart RTL QTimer _eventTimer; QTimer _smartRTLAttemptTimer; SRTL_Reason _srtlReason; bool _vehicleHasLowBattery; bool _lowBatteryHandlingTriggered; bool _executingSmartRTL; // Snake SnakeConnectionStatus _snakeConnectionStatus; bool _snakeCalcInProgress; bool _snakeRecalcNecessary; bool _scenarioDefinedBool; SnakeWorker _snakeWorker; Scenario _scenario; SettingsFact _snakeTileWidth; SettingsFact _snakeTileHeight; SettingsFact _snakeMinTileArea; SettingsFact _snakeLineDistance; SettingsFact _snakeMinTransectLength; ::GeoPoint3D _snakeOrigin; SnakeTiles _snakeTiles; // tiles SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system QVariantList _snakeTileCenterPoints; QNemoProgress _nemoProgress; // measurement progress ROSBridgePtr _pRosBridge; };