Waypoint managers removed, code compiles

qgroundcontrol.pro

@@ -486,16 +486,9 @@ HEADERS += \
     src/Wima/Snake/SnakeTileLocal.h \
     src/Wima/Snake/SnakeTiles.h \
     src/Wima/Snake/SnakeTilesLocal.h \
-    src/Wima/StateMachine.h \
-    src/Wima/WaypointManager/AreaInterface.h \
-    src/Wima/WaypointManager/DefaultManager.h \
-    src/Wima/WaypointManager/EmptyManager.h \
-    src/Wima/WaypointManager/GenericWaypointManager.h \
-    src/Wima/WaypointManager/RTLManager.h \
-    src/Wima/WaypointManager/Settings.h \
-    src/Wima/WaypointManager/Slicer.h \
-    src/Wima/WaypointManager/Utils.h \
+    src/Wima/Utils.h \
     src/Wima/WimaBridge.h \
+    src/Wima/WimaStateMachine.h \
     src/Wima/call_once.h \
     src/api/QGCCorePlugin.h \
     src/api/QGCOptions.h \
@@ -547,6 +540,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) {
 
 SOURCES += \
     src/Vehicle/VehicleEscStatusFactGroup.cc \
+    src/Wima/WimaStateMachine.cpp \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \
     src/api/QGCSettings.cc \
@@ -563,15 +557,7 @@ SOURCES += \
     src/Wima/Snake/NemoInterface.cpp \
     src/Wima/Snake/QNemoProgress.cc \
     src/Wima/Snake/SnakeTile.cpp \
-    src/Wima/StateMachine.cpp \
-    src/Wima/WaypointManager/AreaInterface.cpp \
-    src/Wima/WaypointManager/DefaultManager.cpp \
-    src/Wima/WaypointManager/EmptyManager.cpp \
-    src/Wima/WaypointManager/GenericWaypointManager.cpp \
-    src/Wima/WaypointManager/RTLManager.cpp \
-    src/Wima/WaypointManager/Settings.cpp \
-    src/Wima/WaypointManager/Slicer.cpp \
-    src/Wima/WaypointManager/Utils.cpp \
+    src/Wima/Utils.cpp \
     src/Wima/WimaBridge.cc \
     src/comm/ros_bridge/include/RosBridgeClient.cpp \
     src/comm/ros_bridge/include/com_private.cpp \

src/Wima/Geometry/WimaCorridor.cc

@@ -51,18 +51,6 @@ void WimaCorridor::init()
     this->setObjectName(WimaCorridorName);
 }
 
-void print(const WimaCorridor &area)
-{
-    QString message;
-    print(area, message);
-    qWarning() << message;
-}
-
-void print(const WimaCorridor &area, QString &outputString)
-{
-    print(static_cast<const WimaArea&>(area), outputString);
-}
-
 
 /*!
   \class WimaCorridor

src/Wima/Geometry/WimaCorridor.h

@@ -22,10 +22,6 @@ public:
   // static Members
   static const char *WimaCorridorName;
 
-  // Friends
-  friend void print(const WimaCorridor &area, QString &outputString);
-  friend void print(const WimaCorridor &area);
-
 signals:
 
 public slots:

src/Wima/Geometry/WimaJoinedArea.cc

@@ -50,19 +50,6 @@ void WimaJoinedArea::init()
     this->setObjectName(WimaJoinedAreaName);
 }
 
-void print(const WimaJoinedArea &area)
-{
-    QString message;
-    print(area, message);
-    qWarning() << message;
-}
-
-void print(const WimaJoinedArea &area, QString &outputString)
-{
-    print(static_cast<const WimaArea&>(area), outputString);
-}
-
-
 /*!
   \class WimaJoinedArea
   \brief Joined area (derived from \c WimaArea) composed by the \c WimaMeasurementArea, the \c WimaCorridor, and the \c WimaServiceArea.

src/Wima/Geometry/WimaJoinedArea.h

@@ -25,10 +25,6 @@ public:
     // static Members
     static const char* WimaJoinedAreaName;
 
-    // Friends
-    friend void print(const WimaJoinedArea& area, QString& outputString);
-    friend void print(const WimaJoinedArea& area);
-
 signals:
 
 public slots:

src/Wima/Geometry/WimaServiceArea.cc

@@ -85,18 +85,6 @@ bool WimaServiceArea::loadFromJson(const QJsonObject &json,
   return retVal;
 }
 
-void print(const WimaServiceArea &area) {
-  QString message;
-  print(area, message);
-  qWarning() << message;
-}
-
-void print(const WimaServiceArea &area, QString &outputStr) {
-  print(static_cast<const WimaArea &>(area), outputStr);
-  outputStr.append(QString("Depot Position: %s\n")
-                       .arg(area._depot.toString(QGeoCoordinate::Degrees)));
-}
-
 void WimaServiceArea::init() {
   this->setObjectName(wimaServiceAreaName);
   connect(this, &WimaArea::pathChanged, [this] {

src/Wima/Geometry/WimaServiceArea.h

@@ -26,10 +26,6 @@ public:
   void saveToJson(QJsonObject &json);
   bool loadFromJson(const QJsonObject &json, QString &errorString);
 
-  // Friends
-  friend void print(const WimaServiceArea &area, QString &outputStr);
-  friend void print(const WimaServiceArea &area);
-
   // static Members
   static const char *wimaServiceAreaName;
   static const char *depotLatitudeName;

src/Wima/WaypointManager/AreaInterface.cpp

-#include "AreaInterface.h"
-
-WaypointManager::AreaInterface::AreaInterface()
-    : _mArea(nullptr)
-    , _sArea(nullptr)
-    , _cArea(nullptr)
-    , _jArea(nullptr)
-{
-
-}
-
-WaypointManager::AreaInterface::AreaInterface(WimaMeasurementAreaData *mArea,
-                                              WimaServiceAreaData *sArea,
-                                              WimaCorridorData *corr,
-                                              WimaJoinedAreaData *jArea)
-    : _mArea(mArea)
-    , _sArea(sArea)
-    , _cArea(corr)
-    , _jArea(jArea)
-{
-
-}
-
-void WaypointManager::AreaInterface::setMeasurementArea(WimaMeasurementAreaData *area)
-{
-    _mArea = area;
-}
-
-void WaypointManager::AreaInterface::setServiceArea(WimaServiceAreaData *area)
-{
-    _sArea = area;
-}
-
-void WaypointManager::AreaInterface::setCorridor(WimaCorridorData *area)
-{
-    _cArea = area;
-}
-
-void WaypointManager::AreaInterface::setJoinedArea(WimaJoinedAreaData *area)
-{
-    _jArea = area;
-}
-
-const WimaMeasurementAreaData *WaypointManager::AreaInterface::measurementArea() const
-{
-    return _mArea;
-}
-
-const WimaServiceAreaData *WaypointManager::AreaInterface::serviceArea() const
-{
-    return _sArea;
-}
-
-const WimaCorridorData *WaypointManager::AreaInterface::corridor() const
-{
-    return _cArea;
-}
-
-const WimaJoinedAreaData *WaypointManager::AreaInterface::joinedArea() const
-{
-    return _jArea;
-}
-
-WimaMeasurementAreaData *WaypointManager::AreaInterface::measurementArea()
-{
-    return _mArea;
-}
-
-WimaServiceAreaData *WaypointManager::AreaInterface::serviceArea()
-{
-    return _sArea;
-}
-
-WimaCorridorData *WaypointManager::AreaInterface::corridor()
-{
-    return _cArea;
-}
-
-WimaJoinedAreaData *WaypointManager::AreaInterface::joinedArea()
-{
-    return _jArea;
-}

src/Wima/WaypointManager/AreaInterface.h

-#pragma once
-
-#include "Wima/Geometry/WimaMeasurementAreaData.h"
-#include "Wima/Geometry/WimaServiceAreaData.h"
-#include "Wima/Geometry/WimaCorridorData.h"
-#include "Wima/Geometry/WimaJoinedAreaData.h"
-
-namespace WaypointManager {
-
-class AreaInterface
-{
-public:
-    AreaInterface();
-    AreaInterface(WimaMeasurementAreaData *mArea,
-                  WimaServiceAreaData *sArea,
-                  WimaCorridorData *corr,
-                  WimaJoinedAreaData *jArea);
-
-    void setMeasurementArea (WimaMeasurementAreaData *area);
-    void setServiceArea     (WimaServiceAreaData *area);
-    void setCorridor        (WimaCorridorData *area);
-    void setJoinedArea      (WimaJoinedAreaData *area);
-
-    virtual const WimaMeasurementAreaData   *measurementArea() const;
-    virtual const WimaServiceAreaData       *serviceArea() const;
-    virtual const WimaCorridorData          *corridor() const;
-    virtual const WimaJoinedAreaData        *joinedArea() const;
-
-    virtual WimaMeasurementAreaData *measurementArea();
-    virtual WimaServiceAreaData     *serviceArea();
-    virtual WimaCorridorData        *corridor();
-    virtual WimaJoinedAreaData      *joinedArea();
-
-private:
-    WimaMeasurementAreaData *_mArea;
-    WimaServiceAreaData     *_sArea;
-    WimaCorridorData        *_cArea;
-    WimaJoinedAreaData      *_jArea;
-};
-
-
-} // namespace WaypointManager

src/Wima/WaypointManager/DefaultManager.cpp

-#include "DefaultManager.h"
-#include "Wima/Geometry/GeoUtilities.h"
-#include "Wima/Geometry/PolygonCalculus.h"
-
-#include "MissionSettingsItem.h"
-#include "SimpleMissionItem.h"
-
-WaypointManager::DefaultManager::DefaultManager(Settings &settings,
-                                                AreaInterface &interface)
-    : ManagerBase(settings), _areaInterface(&interface) {}
-
-void WaypointManager::DefaultManager::clear() {
-  _dirty = true;
-  _waypoints.clear();
-  _currentWaypoints.clear();
-  _missionItems.clearAndDeleteContents();
-  _currentMissionItems.clearAndDeleteContents();
-  _waypointsVariant.clear();
-  _currentWaypointsVariant.clear();
-}
-
-bool WaypointManager::DefaultManager::update() {
-  // extract waypoints
-  _currentWaypoints.clear();
-  Slicer::update(_waypoints, _currentWaypoints);
-  return _worker();
-}
-
-bool WaypointManager::DefaultManager::next() {
-  // extract waypoints
-  _currentWaypoints.clear();
-  Slicer::next(_waypoints, _currentWaypoints);
-  return _worker();
-}
-
-bool WaypointManager::DefaultManager::previous() {
-  // extract waypoints
-  _currentWaypoints.clear();
-  Slicer::previous(_waypoints, _currentWaypoints);
-  return _worker();
-}
-
-bool WaypointManager::DefaultManager::reset() {
-  // extract waypoints
-  _currentWaypoints.clear();
-  Slicer::reset(_waypoints, _currentWaypoints);
-  return _worker();
-}
-
-bool WaypointManager::DefaultManager::_insertMissionItem(
-    const QGeoCoordinate &c, size_t index, QmlObjectListModel &list,
-    bool doUpdate) {
-  using namespace WaypointManager::Utils;
-
-  if (!insertMissionItem(c, index /*insertion index*/, list,
-                         _settings->masterController(), _settings->isFlyView(),
-                         &list /*parent*/, doUpdate /*do update*/)) {
-    qWarning(
-        "WaypointManager::DefaultManager::next(): insertMissionItem failed.");
-    Q_ASSERT(false);
-    return false;
-  }
-  return true;
-}
-
-bool WaypointManager::DefaultManager::_insertMissionItem(
-    const QGeoCoordinate &c, size_t index, bool doUpdate) {
-  return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
-}
-
-bool WaypointManager::DefaultManager::_calcShortestPath(
-    const QGeoCoordinate &start, const QGeoCoordinate &destination,
-    QVector<QGeoCoordinate> &path) {
-  using namespace GeoUtilities;
-  using namespace PolygonCalculus;
-  QPolygonF joinedArea2D;
-  toCartesianList(_areaInterface->joinedArea()->coordinateList(),
-                  /*origin*/ start, joinedArea2D);
-  QPointF start2D(0, 0);
-  QPointF end2D;
-  toCartesian(destination, start, end2D);
-  QVector<QPointF> path2DOut;
-
-  bool retVal =
-      PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
-  toGeoList(path2DOut, /*origin*/ start, path);
-
-  return retVal;
-}
-
-bool WaypointManager::DefaultManager::_worker() {
-  // Precondition:
-  // _waypoints must contain valid coordinates.
-  // Slicer must be called befor invoking this function.
-  // E.g. Slicer::reset(_waypoints, _currentWaypoints);
-
-  using namespace WaypointManager::Utils;
-
-  if (_waypoints.count() < 1 || !_settings->valid()) {
-    return false;
-  }
-
-  if (_dirty) {
-    _missionItems.clearAndDeleteContents();
-    _waypointsVariant.clear();
-    // No initialization of _missionItems, don't need MissionSettingsItem here.
-    for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
-      auto &c = _waypoints.at(i);
-      _insertMissionItem(c, _missionItems.count(), _missionItems,
-                         false /*update list*/);
-      _waypointsVariant.push_back(QVariant::fromValue(c));
-    }
-    updateHirarchy(_missionItems);
-    updateSequenceNumbers(
-        _missionItems, 1); // Start with 1, since MissionSettingsItem missing.
-    _dirty = false;
-  }
-
-  _currentMissionItems.clearAndDeleteContents();
-  initialize(_currentMissionItems, _settings->masterController(),
-             _settings->isFlyView());
-
-  // Calculate path from home to first waypoint.
-  QVector<QGeoCoordinate> arrivalPath;
-  if (!_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(),
-                         arrivalPath)) {
-    qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
-             "from home position to first waypoint.");
-    qWarning() << "from: " << _settings->homePosition();
-    qWarning() << "to: " << _currentWaypoints.first();
-    return false;
-  }
-  if (!arrivalPath.empty())
-    arrivalPath.removeFirst();
-  if (!arrivalPath.empty())
-    arrivalPath.removeLast();
-
-  // calculate path from last waypoint to home
-  QVector<QGeoCoordinate> returnPath;
-  if (!_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(),
-                         returnPath)) {
-    qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
-             "from home position to last waypoint.");
-    qWarning() << "from: " << _currentWaypoints.last();
-    qWarning() << "to: " << _settings->homePosition();
-    return false;
-  }
-  if (!returnPath.empty())
-    returnPath.removeFirst();
-  if (!returnPath.empty())
-    returnPath.removeLast();
-
-  // Create mission items.
-  // Set home position of MissionSettingsItem.
-  MissionSettingsItem *settingsItem =
-      _currentMissionItems.value<MissionSettingsItem *>(0);
-  if (settingsItem == nullptr) {
-    Q_ASSERT(false);
-    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
-    return false;
-  }
-  settingsItem->setCoordinate(_settings->homePosition());
-
-  // First mission item is takeoff command.
-  _insertMissionItem(_settings->homePosition(), 1 /*insertion index*/,
-                     false /*do update*/);
-  SimpleMissionItem *takeoffItem =
-      _currentMissionItems.value<SimpleMissionItem *>(1);
-  if (takeoffItem == nullptr) {
-    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
-    Q_ASSERT(takeoffItem != nullptr);
-    return false;
-  }
-  makeTakeOffCmd(takeoffItem, _settings->masterController());
-
-  // Create change speed item.
-  _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/,
-                     false /*do update*/);
-  SimpleMissionItem *speedItem =
-      _currentMissionItems.value<SimpleMissionItem *>(2);
-  if (speedItem == nullptr) {
-    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
-    Q_ASSERT(speedItem != nullptr);
-    return false;
-  }
-  makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
-
-  // insert arrival path
-  for (auto coordinate : arrivalPath) {
-    _insertMissionItem(coordinate,
-                       _currentMissionItems.count() /*insertion index*/,
-                       false /*do update*/);
-  }
-
-  // Create change speed item (after arrival path).
-  int index = _currentMissionItems.count();
-  _insertMissionItem(_currentWaypoints.first(), index /*insertion index*/,
-                     false /*do update*/);
-  speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
-  if (speedItem == nullptr) {
-    Q_ASSERT(false);
-    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
-    return false;
-  }
-  makeSpeedCmd(speedItem, _settings->flightSpeed());
-
-  // Insert slice.
-  for (auto coordinate : _currentWaypoints) {
-    _insertMissionItem(coordinate,
-                       _currentMissionItems.count() /*insertion index*/,
-                       false /*do update*/);
-  }
-
-  // Create change speed item, after slice.
-  index = _currentMissionItems.count();
-  _insertMissionItem(_currentWaypoints.last(), index /*insertion index*/,
-                     false /*do update*/);
-  speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
-  if (speedItem == nullptr) {
-    Q_ASSERT(false);
-    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
-    return false;
-  }
-  makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
-
-  // Insert return path coordinates.
-  for (auto coordinate : returnPath) {
-    _insertMissionItem(coordinate,
-                       _currentMissionItems.count() /*insertion index*/,
-                       false /*do update*/);
-  }
-
-  // Set land command for last mission item.
-  index = _currentMissionItems.count();
-  _insertMissionItem(_settings->homePosition(), index /*insertion index*/,
-                     false /*do update*/);
-  SimpleMissionItem *landItem =
-      _currentMissionItems.value<SimpleMissionItem *>(index);
-  if (landItem == nullptr) {
-    Q_ASSERT(false);
-    qWarning("WaypointManager::DefaultManager::next(): nullptr.");
-    return false;
-  }
-  makeLandCmd(landItem, _settings->masterController());
-
-  // Set altitude.
-  for (int i = 1; i < _currentMissionItems.count(); ++i) {
-    SimpleMissionItem *item =
-        _currentMissionItems.value<SimpleMissionItem *>(i);
-    if (item == nullptr) {
-      Q_ASSERT(false);
-      qWarning("WimaController::updateAltitude(): nullptr");
-      return false;
-    }
-    item->altitude()->setRawValue(_settings->altitude());
-  }
-
-  // Update list _currentMissionItems.
-  updateHirarchy(_currentMissionItems);
-  updateSequenceNumbers(_currentMissionItems);
-
-  // Prepend arrival path to slice.
-  for (long i = arrivalPath.size() - 1; i >= 0; --i)
-    _currentWaypoints.push_front(arrivalPath[i]);
-  _currentWaypoints.push_front(_settings->homePosition());
-  // Append return path to slice.
-  for (auto c : returnPath)
-    _currentWaypoints.push_back(c);
-  _currentWaypoints.push_back(_settings->homePosition());
-
-  // Create variant list.
-  _currentWaypointsVariant.clear();
-  for (auto c : _currentWaypoints)
-    _currentWaypointsVariant.push_back(QVariant::fromValue(c));
-
-  return true;
-}

src/Wima/WaypointManager/DefaultManager.h

-#pragma once
-
-#include <QGeoCoordinate>
-#include <QVector>
-
-#include "GenericWaypointManager.h"
-#include "QmlObjectListModel.h"
-#include "Settings.h"
-#include "AreaInterface.h"
-
-namespace WaypointManager {
-
-typedef GenericWaypointManager<QGeoCoordinate,
-                               QVector,
-                               QmlObjectListModel,
-                               Settings>
-        ManagerBase;
-//!
-//! \brief The DefaultManager class is used to manage SimpleMissionItems.
-//!
-//! @note The \class QmlObjectList returned by \fn missionItems doesn't contain a MissionSettingsItem.
-//! This list is supposed to be used for display purposes only.
-class DefaultManager : public ManagerBase
-{
-public:
-    DefaultManager() = delete;
-    DefaultManager(Settings &settings,
-                   AreaInterface &interface);
-
-    void clear() override;
-
-
-    virtual bool update() override;
-    virtual bool next() override;
-    virtual bool previous() override;
-    virtual bool reset() override;
-
-protected:
-    bool _insertMissionItem(const QGeoCoordinate &c,
-                            size_t index,
-                            QmlObjectListModel &list,
-                            bool doUpdate);
-    bool _insertMissionItem(const QGeoCoordinate &c,
-                            size_t index,
-                            bool doUpdate);
-    bool _calcShortestPath(const QGeoCoordinate &start,
-                           const QGeoCoordinate &destination,
-                           QVector<QGeoCoordinate> &path);
-
-    AreaInterface *_areaInterface;
-private:
-    bool _worker();
-
-};
-
-} // namespace WaypointManager

src/Wima/WaypointManager/EmptyManager.cpp

-#include "EmptyManager.h"
-
-namespace WaypointManager {
-
-WaypointManager::EmptyManager::EmptyManager(Settings &settings, AreaInterface &)
-    : ManagerBase(settings) {}
-
-void WaypointManager::EmptyManager::clear() {}
-
-bool WaypointManager::EmptyManager::update() { return true; }
-
-bool WaypointManager::EmptyManager::next() { return true; }
-
-bool WaypointManager::EmptyManager::previous() { return true; }
-
-bool WaypointManager::EmptyManager::reset() { return true; }
-
-} // namespace WaypointManager

src/Wima/WaypointManager/EmptyManager.h

-#pragma once
-
-#include <QGeoCoordinate>
-#include <QVector>
-
-#include "AreaInterface.h"
-#include "GenericWaypointManager.h"
-#include "QmlObjectListModel.h"
-#include "Settings.h"
-
-namespace WaypointManager {
-
-typedef GenericWaypointManager<QGeoCoordinate, QVector, QmlObjectListModel,
-                               Settings>
-    ManagerBase;
-
-//!
-//! \brief The EmptyManager is a place holder and does noting.
-class EmptyManager : public ManagerBase {
-public:
-  EmptyManager() = delete;
-  EmptyManager(Settings &settings, AreaInterface &);
-
-  void clear() override;
-  virtual bool update() override;
-  virtual bool next() override;
-  virtual bool previous() override;
-  virtual bool reset() override;
-};
-
-} // namespace WaypointManager

src/Wima/WaypointManager/GenericWaypointManager.cpp

-#include "GenericWaypointManager.h"

src/Wima/WaypointManager/GenericWaypointManager.h

-#pragma once
-
-#include <iostream>
-
-#include "Slicer.h"
-
-namespace WaypointManager {
-
-template<class WaypointType,
-         template<class, class...> class ContainerType,
-         class MissionItemList,
-         class SettingsType>
-class GenericWaypointManager : public Slicer
-{
-public:
-    typedef ContainerType<WaypointType>  WaypointList;
-
-    GenericWaypointManager() = delete;
-    GenericWaypointManager(SettingsType &settings);