#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) { return _container; } QmlObjectListModel* missionItems (void); QmlObjectListModel* currentMissionItems (void); QVariantList waypointPath (void); QVariantList currentWaypointPath (void); Fact* enableWimaController (void) { return &_enableWimaController; } Fact* overlapWaypoints (void) { return &_overlapWaypoints; } Fact* maxWaypointsPerPhase (void) { return &_maxWaypointsPerPhase; } Fact* startWaypointIndex (void) { return &_nextPhaseStartWaypointIndex; } Fact* showAllMissionItems (void) { return &_showAllMissionItems; } Fact* showCurrentMissionItems(void) { return &_showCurrentMissionItems; } Fact* flightSpeed (void) { return &_flightSpeed; } Fact* arrivalReturnSpeed (void) { return &_arrivalReturnSpeed; } Fact* altitude (void) { return &_altitude; } 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(); // wrapper for _checkSmartRTLPreCondition(QString &errorString) Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)# Q_INVOKABLE void executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring) 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; }; /* * The following explains the structure of * _missionController.visualItems(). * * Index Description * -------------------------------------------- * 0 MissionSettingsItem * 1 Takeoff Command * 2 Speed Command: arrivalReturnSpeed * 3 Arrival Path Waypoint 0 * ... * 3+n-1 Arrival Path Waypoint n-1 * 3+n Speed Command: flightSpeed * 3+n+1 Circular Survey Waypoint 0 * ... * 3+n+m Circular Survey Waypoint m-1 * 3+n+m+1 Speed Command: arrivalReturnSpeed * 3+n+m+2 Return Path Waypoint 0 * ... * 3+n+m+2+l Return Path Waypoint l-1 * 3+n+m+2+l+1 Land command * * _currentMissionItems is equal to * _missionController.visualItems() except that it * is missing the MissionSettingsItem */ template CoordinateType getCoordinate(const SimpleMissionItem* item){ return item->coordinate(); } template<> QVariant getCoordinate(const SimpleMissionItem* item); /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList. template class ContainerType> bool getCoordinates(QmlObjectListModel &missionItems, ContainerType &coordinateList, std::size_t startIndex, std::size_t endIndex){ if ( startIndex < std::size_t(missionItems.count()) && endIndex < std::size_t(missionItems.count()) ) { if (startIndex > endIndex) { if (!getCoordinates(missionItems, coordinateList, startIndex, missionItems.count()-1)) return false; if (!getCoordinates(missionItems, coordinateList, 0, endIndex)) return false; } else { for (std::size_t i = startIndex; i <= endIndex; i++) { SimpleMissionItem *mItem = missionItems.value(int(i)); if (mItem == nullptr) { coordinateList.clear(); return false; } coordinateList.append(getCoordinate(mItem)); } } } else return false; return true; } /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList. template class ContainerType> bool getCoordinates(QmlObjectListModel &missionItems, ContainerType &coordinateList){ return getCoordinates(missionItems, coordinateList, 0, missionItems.count()); }