#pragma once #include #include #include "QGCMapPolygon.h" #include "QmlObjectListModel.h" #include "WimaArea.h" #include "WimaMeasurementArea.h" #include "WimaServiceArea.h" #include "WimaCorridor.h" #include "WimaDataContainer.h" #include "WimaMeasurementAreaData.h" #include "WimaCorridorData.h" #include "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 "SnakeWorker.h" #include "SnakeTiles.h" #include "SnakeTilesLocal.h" #include "GeoPoint3D.h" #include "QNemoProgress.h" #include "ros_bridge/include/ROSBridge.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(Fact* reverse READ reverse 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) { return _masterController; } MissionController* missionController (void) { return _missionController; } QmlObjectListModel* visualItems (void) { return &_visualItems; } // 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) { return &_missionItems; } QmlObjectListModel* currentMissionItems (void) { return &_currentMissionItems; } QVariantList waypointPath (void) const; QVariantList currentWaypointPath (void) { return _currentWaypointPath; } 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* reverse (void) { return &_reverse; } 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* reverseName; 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 _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 _reverseChangedHandler (); 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 _visualItems; // 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; 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 _reverse; // Reverses the phase direction. Phases go from high to low waypoint numbers, if true. 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()); }