Working on default waypointmanager, not done yet.

qgroundcontrol.pro

@@ -438,12 +438,14 @@ HEADERS += \
     src/Wima/Snake/SnakeTiles.h \
     src/Wima/Snake/SnakeTilesLocal.h \
     src/Wima/Snake/SnakeWorker.h \
+    src/Wima/WaypointManager/AreaInterface.h \
+    src/Wima/WaypointManager/DefaultManager.h \
     src/Wima/WaypointManager/GenericSlicer.h \
     src/Wima/WaypointManager/GenericWaypointManager.h \
     src/Wima/Geometry/WimaPolygonArray.h \
     src/Wima/Snake/snaketile.h \
     src/Wima/WaypointManager/Settings.h \
-    src/Wima/WaypointManager/utils.h \
+    src/Wima/WaypointManager/Utils.h \
     src/api/QGCCorePlugin.h \
     src/api/QGCOptions.h \
     src/api/QGCSettings.h \
@@ -497,10 +499,12 @@ SOURCES += \
     src/Wima/Geometry/PolygonArray.cc \
     src/Wima/Snake/QNemoProgress.cc \
     src/Wima/Snake/SnakeWorker.cc \
+    src/Wima/WaypointManager/AreaInterface.cpp \
+    src/Wima/WaypointManager/DefaultManager.cpp \
     src/Wima/WaypointManager/GenericSlicer.cpp \
     src/Wima/WaypointManager/GenericWaypointManager.cpp \
     src/Wima/WaypointManager/Settings.cpp \
-    src/Wima/WaypointManager/utils.cpp \
+    src/Wima/WaypointManager/Utils.cpp \
     src/comm/ros_bridge/include/ComPrivateInclude.cpp \
     src/comm/ros_bridge/include/MessageTag.cpp \
     src/comm/ros_bridge/include/TopicPublisher.cpp \

src/Wima/CircularSurveyComplexItem.h

@@ -2,11 +2,11 @@
 
 #include "TransectStyleComplexItem.h"
 
-#include "PolygonCalculus.h"
-#include "PlanimetryCalculus.h"
-#include "GeoUtilities.h"
+#include "Geometry/PolygonCalculus.h"
+#include "Geometry/PlanimetryCalculus.h"
+#include "Geometry/GeoUtilities.h"
 #include "QVector"
-#include "Circle.h"
+#include "Geometry/Circle.h"
 #include "SettingsFact.h"
 
 class CircularSurveyComplexItem :public TransectStyleComplexItem

src/Wima/Geometry/PolygonCalculus.cc

 #include "PolygonCalculus.h"
 #include "PlanimetryCalculus.h"
-#include "OptimisationTools.h"
+#include "Wima/OptimisationTools.h"
 
 #include <QVector3D>
 

src/Wima/Geometry/WimaArea.h

@@ -4,7 +4,6 @@
 #include "QGCMapPolyline.h"
 #include "Vehicle.h"
 #include "qobject.h"
-#include "WimaVehicle.h"
 #include <QLineF>
 #include <QPointF>
 #include "QGCGeo.h"

src/Wima/Geometry/WimaAreaData.cc

@@ -2,6 +2,7 @@
 
 WimaAreaData::WimaAreaData(QObject *parent)
     : QObject(parent)
+    , _listValid(false)
 {
     _maxAltitude = 0;
 }
@@ -31,14 +32,15 @@ QGeoCoordinate WimaAreaData::center() const
     return _center;
 }
 
-QList<QGeoCoordinate> WimaAreaData::coordinateList() const
+const QList<QGeoCoordinate> &WimaAreaData::coordinateList() const
 {
-    QList<QGeoCoordinate> coordinateList;
-
-    for ( auto coorinate : _path)
-        coordinateList.append(coorinate.value<QGeoCoordinate>());
+    static QList<QGeoCoordinate> list;
+    if ( !_listValid ) {
+        for ( auto coorinate : _path)
+            list.append(coorinate.value<QGeoCoordinate>());
+    }
 
-    return coordinateList;
+    return list;
 }
 
 bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const
@@ -54,6 +56,16 @@ bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const
         return false;
 }
 
+void WimaAreaData::append(const QGeoCoordinate &c) {
+    _listValid = false;
+    _path.push_back(QVariant::fromValue(c));
+}
+
+void WimaAreaData::clear() {
+    _listValid = false;
+    _path.clear();
+}
+
 
 /*!
  * \fn void WimaAreaData::setMaxAltitude(double maxAltitude)
@@ -72,6 +84,7 @@ void WimaAreaData::setMaxAltitude(double maxAltitude)
 
 void WimaAreaData::setPath(const QVariantList &coordinateList)
 {
+    _listValid = false;
     _path.clear();
     _path.append(coordinateList);
 }
@@ -113,6 +126,7 @@ void WimaAreaData::assign(const WimaArea &other)
  */
 void WimaAreaData::setPath(const QList<QGeoCoordinate> &coordinateList)
 {
+    _listValid = false;
     _path.clear();
 
     // copy all coordinates to _path

src/Wima/Geometry/WimaAreaData.h

@@ -21,12 +21,12 @@ public:
     double                  maxAltitude()           const;
     QVariantList            path()                  const;
     QGeoCoordinate          center()                const;
-    QList<QGeoCoordinate>   coordinateList()        const;
+    const QList<QGeoCoordinate> &coordinateList() const;
     bool                    containsCoordinate(const QGeoCoordinate &coordinate)    const;
     virtual QString         type() const = 0;
 
-    void append(const QGeoCoordinate &c)    {_path.push_back(QVariant::fromValue(c));}
-    void clear()                            {_path.clear();}
+    void append(const QGeoCoordinate &c);
+    void clear();
 
 signals:
     void maxAltitudeChanged (double maxAltitude);
@@ -51,5 +51,6 @@ private:
     double         _maxAltitude;
     QVariantList   _path;
     QGeoCoordinate _center;
+    bool           _listValid;
 };
 

src/Wima/Geometry/WimaMeasurementArea.h

@@ -51,7 +51,6 @@ signals:
     void bottomLayerAltitudeChanged     (void);
     void numberOfLayersChanged          (void);
     void layerDistanceChanged           (void);
-    void vehicleCorridorChanged         (WimaVCorridor* corridor);
 
 public slots:
     void setBottomLayerAltitude (double altitude);

src/Wima/Geometry/WimaServiceArea.h

@@ -38,7 +38,6 @@ public:
 signals:
     void takeOffPositionChanged         (void);
     void landPositionChanged            (void);
-    void vehicleCorridorChanged         (WimaVCorridor& corridor);
 
 public slots:
     void setTakeOffPosition             (const QGeoCoordinate& coordinate);

src/Wima/Snake/SnakeTiles.h

 #pragma once
 #include "snaketile.h"
-#include "WimaPolygonArray.h"
+#include "Wima/Geometry/WimaPolygonArray.h"
 
 typedef WimaPolygonArray<SnakeTile, QVector> SnakeTiles;

src/Wima/Snake/SnakeTilesLocal.h

 #pragma once
 
 #include "ros_bridge/include/MessageGroups.h"
-#include "Polygon2D.h"
-#include "WimaPolygonArray.h"
+#include "Wima/Geometry/Polygon2D.h"
+#include "Wima/Geometry/WimaPolygonArray.h"
 
 namespace  MsgGroups = ROSBridge::MessageGroups;
 typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> SnakeTilesLocal;

src/Wima/Snake/snaketile.h

 #pragma once
 
-#include "WimaAreaData.h"
+#include "Wima/Geometry/WimaAreaData.h"
 
 class SnakeTile : public WimaAreaData
 {

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 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"
+
+bool WaypointManager::DefaultManager::update()
+{
+    // extract waypoints
+    _slice.clear();
+    _slicer->update(_waypoints, _slice);
+    return _worker();
+}
+
+bool WaypointManager::DefaultManager::next()
+{
+    // extract waypoints
+    _slice.clear();
+    _slicer->next(_waypoints, _slice);
+    return _worker();
+}
+
+bool WaypointManager::DefaultManager::previous()
+{
+    // extract waypoints
+    _slice.clear();
+    _slicer->previous(_waypoints, _slice);
+    return _worker();
+}
+
+bool WaypointManager::DefaultManager::reset()
+{
+    // extract waypoints
+    _slice.clear();
+    _slicer->reset(_waypoints, _slice);
+    return _worker();
+}
+
+bool WaypointManager::DefaultManager::_insertMissionItem(size_t index, const QGeoCoordinate &c, bool doUpdate)
+{
+    using namespace WaypointManager::Utils;
+
+    if ( !insertMissionItem(c,
+                            index /*insertion index*/,
+                            _missionItems,
+                            _settings->vehicle(),
+                            _settings->isFlyView(),
+                            &_missionItems /*parent*/,
+                            doUpdate /*do update*/) )
+    {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed.");
+        return false;
+    }
+    return true;
+}
+
+bool WaypointManager::DefaultManager::_calcShortestPath(
+        const QGeoCoordinate &start,
+        const QGeoCoordinate &destination,
+        QVector<QGeoCoordinate> &path)
+{
+        using namespace GeoUtilities;
+        using namespace PolygonCalculus;
+        QVector<QPointF> path2D;
+        bool retVal = PolygonCalculus::shortestPath(
+                                       toQPolygonF(toCartesian2D(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start)),
+                                       /*start point*/ QPointF(0,0),
+                                       /*destination*/ toCartesian2D(destination, start),
+                                       /*shortest path*/ path2D);
+        path.append(toGeo(path2D, /*origin*/ start));
+
+        return  retVal;
+}
+
+bool WaypointManager::DefaultManager::_worker()
+{
+    using namespace WaypointManager::Utils;
+
+    if (_waypoints.count() < 1) {
+        return false;
+    }
+
+    _missionItems.clearAndDeleteContents();
+    initialize(_missionItems, _settings->vehicle(), _settings->isFlyView());
+
+    // calculate path from home to first waypoint
+    QVector<QGeoCoordinate> arrivalPath;
+    if (!_settings->homePosition().isValid()){
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
+        return false;
+    }
+    if ( !_calcShortestPath(_settings->homePosition(), _slice.first(), arrivalPath) ) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
+        return false;
+    }
+    arrivalPath.removeFirst();
+
+    // calculate path from last waypoint to home
+    QVector<QGeoCoordinate> returnPath;
+    if ( !_calcShortestPath(_slice.last(), _settings->homePosition(), returnPath) ) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint.");
+        return false;
+    }
+    returnPath.removeFirst();
+    returnPath.removeLast();
+
+
+
+    // Create mission items.
+    // Set home position of MissionSettingsItem.
+    MissionSettingsItem* settingsItem = _missionItems.value<MissionSettingsItem *>(0);
+    if (settingsItem == nullptr) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
+        return false;
+    }
+    settingsItem->setCoordinate(_settings->homePosition());
+
+    // Set takeoff position for first mission item (bug).
+    _insertMissionItem(1 /*insertion index*/, _settings->homePosition(), false /*do update*/);
+    SimpleMissionItem *takeoffItem = _missionItems.value<SimpleMissionItem*>(1);
+    if (takeoffItem == nullptr) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
+        return false;
+    }
+    takeoffItem->setCoordinate(_settings->homePosition());
+
+    // Create change speed item.
+    _insertMissionItem(2 /*insertion index*/, _settings->homePosition(), false /*do update*/);
+    SimpleMissionItem *speedItem = _missionItems.value<SimpleMissionItem*>(2);
+    if (speedItem == nullptr) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
+        return false;
+    }
+    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
+    // Set coordinate must be after setCommand (setCommand sets coordinate to zero).
+    speedItem->setCoordinate(_settings->homePosition());
+    speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());
+
+    // insert arrival path
+    for (auto coordinate : arrivalPath) {
+        _insertMissionItem(_missionItems.count() /*insertion index*/,
+                           coordinate,
+                           false /*do update*/);
+    }
+
+    // Create change speed item (after arrival path).
+    int index = _missionItems.count();
+    _insertMissionItem(index /*insertion index*/, _slice.first(), false /*do update*/);
+    speedItem = _missionItems.value<SimpleMissionItem*>(index);
+    if (speedItem == nullptr) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
+        return false;
+    }
+    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
+    speedItem->setCoordinate(_slice.first());
+    speedItem->missionItem().setParam2(_settings->flightSpeed());
+
+    // Insert slice.
+    for (auto coordinate : _slice) {
+        _insertMissionItem(_missionItems.count() /*insertion index*/,
+                           coordinate,
+                           false /*do update*/);
+    }
+
+    // Create change speed item, after slice.
+    index = _missionItems.count();
+    _insertMissionItem(index /*insertion index*/, _slice.last(), false /*do update*/);
+    speedItem = _missionItems.value<SimpleMissionItem*>(index);
+    if (speedItem == nullptr) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
+        return false;
+    }
+    speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
+    speedItem->setCoordinate(_slice.last());
+    speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());
+
+    // Insert return path coordinates.
+    for (auto coordinate : returnPath) {
+        _insertMissionItem(_missionItems.count() /*insertion index*/,
+                           coordinate,
+                           false /*do update*/);
+    }
+
+    // Set land command for last mission item.
+    index = _missionItems.count();
+    _insertMissionItem(index /*insertion index*/, _settings->homePosition(), false /*do update*/);
+    SimpleMissionItem *landItem = _missionItems.value<SimpleMissionItem*>(index);
+    if (landItem == nullptr) {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): nullptr.");
+        return false;
+    }
+
+    MAV_CMD landCmd = _settings->vehicle()->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
+    if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
+        landItem->setCommand(landCmd);
+    } else {
+        assert(false);
+        qWarning("WaypointManager::DefaultManager::next(): Land command not supported!");
+        return false;
+    }
+
+    // Prepend arrival path to slice.
+    for ( long i = arrivalPath.size()-1; i >=0; --i )
+        _slice.push_front(arrivalPath[i]);
+    // Append return path to slice.
+    for ( auto c : returnPath )
+        _slice.push_back(c);
+
+
+    // Set altitude.
+    for (int i = 0; i < _missionItems.count(); ++i) {
+        SimpleMissionItem *item = _missionItems.value<SimpleMissionItem *>(i);
+        if (item == nullptr) {
+            assert(false);
+            qWarning("WimaController::updateAltitude(): nullptr");
+            return false;
+        }
+        item->altitude()->setRawValue(_settings->altitude());
+    }
+
+    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;
+
+class DefaultManager : public ManagerBase
+{
+public:
+    DefaultManager() = delete;
+    DefaultManager(Slicer *slicer,
+                   Settings *settings,
+                   AreaInterface *interface);
+
+
+    virtual bool update() override;
+    virtual bool next() override;
+    virtual bool previous() override;
+    virtual bool reset() override;
+
+protected:
+    bool _insertMissionItem(size_t index, const QGeoCoordinate &c, bool doUpdate);
+    bool _calcShortestPath(const QGeoCoordinate &start,
+                           const QGeoCoordinate &destination,
+                           QVector<QGeoCoordinate> &path);
+
+    AreaInterface *_areaInterface;
+private:
+    bool _worker();
+
+};
+
+} // namespace WaypointManager

src/Wima/WaypointManager/GenericSlicer.h

@@ -3,7 +3,7 @@
 #include <assert.h>
 #include <iostream>
 
-#include "utils.h"
+#include "Utils.h"
 
 //! @brief Base class for all waypoint managers.
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
@@ -15,10 +15,10 @@ public:
 
 
     // Slicing.
-    void slice      (const ContainerType &source, ContainerType &slice);
-    void next       (const ContainerType &source, ContainerType &slice);
-    void previous   (const ContainerType &source, ContainerType &slice);
-    void reset      (const ContainerType &source, ContainerType &slice);
+    void update     (const ContainerType &source, ContainerType &update);
+    void next       (const ContainerType &source, ContainerType &update);
+    void previous   (const ContainerType &source, ContainerType &update);
+    void reset      (const ContainerType &source, ContainerType &update);
 
 
     // Slicing parameters.
@@ -134,7 +134,7 @@ void GenericSlicer<ElementType, Container>::_updateIdx(std::size_t size)
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericSlicer<ElementType, Container>::slice(const ContainerType &source,
+void GenericSlicer<ElementType, Container>::update(const ContainerType &source,
                                                   Container<ElementType> &slice){
 
     if ( !_idxValid)

src/Wima/WaypointManager/GenericWaypointManager.h

@@ -3,59 +3,186 @@
 #include <iostream>
 
 #include "GenericSlicer.h"
-#include "Settings.h"
 
 namespace WaypointManager {
 
 template<class WaypointType,
-         template<class, class...> class ContainerType>
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
 class GenericWaypointManager
 {
-    typedef GenericSlicer<WaypointType, ContainerType> Slicer;
 public:
+    typedef GenericSlicer<WaypointType, ContainerType> Slicer;
     typedef ContainerType<WaypointType>  WaypointList;
 
     GenericWaypointManager() = delete;
-    GenericWaypointManager(Slicer *slicer, Settings *settings);
+    GenericWaypointManager(Slicer *slicer, SettingsType *settings);
 
     // Waypoint editing.
     void            setWaypoints(const WaypointList &waypoints);
     void            push_back   (const WaypointType &wp);
     void            push_front  (const WaypointType &wp);
     void            clear       ();
-    void            insert      (int i, const ElementType &wp);
-    uint32_t        size        ()  const;
-    WaypointType    &at         (unsigned int i);
+    void            insert      (std::size_t i, const WaypointType &wp);
+    std::size_t     size        ()  const;
+    WaypointType    &at         (std::size_t i);
+
+
+    const WaypointList      &waypoints() const;
+    const WaypointList      &slice() const;
+    const MissionItemList   &missionItems() const;
+
+    virtual bool update() = 0;
+    virtual bool next() = 0;
+    virtual bool previous() = 0;
+    virtual bool reset() = 0;
+
+protected:
+    WaypointList        _waypoints;
+    WaypointList        _slice;
+    MissionItemList     _missionItems;
+    Slicer             *_slicer;
+    SettingsType       *_settings;
+};
 
 
-    const ContainerType &getWaypoints  () ;
-    const ContainerType &getSlcieaypoints  () ;
-    const ContainerType &missionItems  () ;
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+GenericWaypointManager<WaypointType,
+                       ContainerType,
+                       MissionItemList,
+                       SettingsType>::GenericWaypointManager(
+        Slicer *slicer,
+        SettingsType *Settings
+        )
+  : _slicer(slicer)
+  , _settings(_settings)
+{}
 
-    void slice();
-    void next();
-    void previous();
-    void reset();
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+void GenericWaypointManager<WaypointType,
+                            ContainerType,
+                            MissionItemList,
+                            SettingsType>::push_back(const WaypointType &wp)
+{
+    _waypoints.push_back(wp);
+}
 
-private:
-    WaypointList _waypoints;
-    WaypointList _slice;
-    bool         _sliceValid;
-    Slicer      *_slicer;
-    Settings    *_settings;
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+void GenericWaypointManager<WaypointType,
+                            ContainerType,
+                            MissionItemList,
+                            SettingsType>::push_front(
+        const WaypointType &wp)
+{
+    _waypoints.push_front(wp);
+}
 
-};
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+void GenericWaypointManager<WaypointType,
+                            ContainerType,
+                            MissionItemList,
+                            SettingsType>::clear()
+{
+    _waypoints.clear();
+}
 
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+void GenericWaypointManager<WaypointType,
+                            ContainerType,
+                            MissionItemList,
+                            SettingsType>::insert(std::size_t i,
+                                                  const WaypointType &wp)
+{
+    _waypoints.insert(i, wp);
+}
 
 template<class WaypointType,
-         template<class, class...> class ContainerType>
-GenericWaypointManager<WaypointType, ContainerType>::GenericWaypointManager(
-        Slicer *slicer,
-        Settings *Settings) :
-    _sliceValid(false)
-  , _slicer(slicer)
-  , _settings(_settings)
-{}
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+std::size_t GenericWaypointManager<WaypointType,
+                                   ContainerType,
+                                   MissionItemList,
+                                   SettingsType>::size() const
+{
+    return _waypoints.size();
+}
+
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+WaypointType & GenericWaypointManager<WaypointType,
+                                      ContainerType,
+                                      MissionItemList,
+                                      SettingsType>::at(std::size_t i)
+{
+    return _waypoints.at(i);
+}
+
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+const ContainerType<WaypointType>  &GenericWaypointManager<WaypointType,
+                                                           ContainerType,
+                                                           MissionItemList,
+                                                           SettingsType>::waypoints() const
+{
+    return _waypoints;
+}
+
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+const ContainerType<WaypointType> &GenericWaypointManager<WaypointType,
+                                                          ContainerType,
+                                                          MissionItemList,
+                                                          SettingsType>::slice() const
+{
+    return _slice;
+}
+
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+const MissionItemList &GenericWaypointManager<WaypointType,
+                                              ContainerType,
+                                              MissionItemList,
+                                              SettingsType>::missionItems() const
+{
+    return _missionItems;
+}
+
+template<class WaypointType,
+         template<class, class...> class ContainerType,
+         class MissionItemList,
+         class SettingsType>
+void GenericWaypointManager<WaypointType,
+                            ContainerType,
+                            MissionItemList,
+                            SettingsType>::setWaypoints(const ContainerType<WaypointType> &waypoints)
+{
+    _waypoints = waypoints;
+}
 
 
 

src/Wima/WaypointManager/Settings.cpp

 #include "Settings.h"
+
+void WaypointManager::Settings::setHomePosition(QGeoCoordinate &c)
+{
+    _homePosition = c;
+}
+void WaypointManager::Settings::setVehicle(Vehicle *vehicle)
+{
+    _vehicle = vehicle;
+}
+
+void WaypointManager::Settings::setIsFlyView(bool isFlyView)
+{
+    _isFlyView = isFlyView;
+}
+
+void WaypointManager::Settings::setArrivalReturnSpeed(double speed)
+{
+    if (speed > 0)
+        _arrivalReturnSpeed = speed;
+}
+
+void WaypointManager::Settings::setFlightSpeed(double speed)
+{
+    if ( speed > 0)
+        _flightSpeed = speed;
+}
+
+void WaypointManager::Settings::setAltitude(double altitude)
+{
+    if ( altitude > 0)
+        _altitude = altitude;
+}
+
+const QGeoCoordinate &WaypointManager::Settings::homePosition() const
+{
+    return _homePosition;
+}
+
+Vehicle *WaypointManager::Settings::vehicle() const
+{
+    return _vehicle;
+}
+
+bool WaypointManager::Settings::isFlyView() const
+{
+    return _isFlyView;
+}
+
+double WaypointManager::Settings::arrivalReturnSpeed() const
+{
+    return _arrivalReturnSpeed;
+}
+
+double WaypointManager::Settings::flightSpeed() const
+{
+    return _flightSpeed;
+}
+
+double WaypointManager::Settings::altitude() const
+{
+    return _altitude;
+}

src/Wima/WaypointManager/Settings.h

 #pragma once
 
+#include <QGeoCoordinate>
+
+#include "Vehicle.h"
+
 namespace WaypointManager {
 
 class Settings
 {
 public:
     Settings();
+
+    void setHomePosition(QGeoCoordinate &c);
+    void setVehicle(Vehicle *vehicle);
+    void setIsFlyView(bool isFlyView);
+    void setArrivalReturnSpeed(double speed);
+    void setFlightSpeed(double speed);
+    void setAltitude(double altitude);
+
+    const QGeoCoordinate &homePosition() const;
+    Vehicle *vehicle() const;
+    bool isFlyView() const;
+    double arrivalReturnSpeed() const;
+    double flightSpeed() const;
+    double altitude() const;
+
+private:
+    QGeoCoordinate _homePosition;
+    Vehicle       *_vehicle;
+    bool           _isFlyView;
+    double         _arrivalReturnSpeed;
+    double         _flightSpeed;
+    double         _altitude;
+
 };
 
 } // namespace WaypointManager

src/Wima/WaypointManager/Utils.cpp

+#include "Utils.h"
+#include "MissionSettingsItem.h"
+
+
+template<>
+QVariant WaypointManager::Utils::getCoordinate(const SimpleMissionItem* item)
+{
+    return QVariant::fromValue(item->coordinate());
+}
+
+bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
+                                               long index,
+                                               QmlObjectListModel &list,
+                                               Vehicle* vehicle,
+                                               bool flyView,
+                                               QObject *parent,
+                                               bool doUpdates)
+{
+    using namespace WaypointManager::Utils;
+
+    if ( !coordinate.isValid() ){
+        return false;
+    } else {
+        if (parent == nullptr)
+            parent = &list;
+        SimpleMissionItem * newItem = new SimpleMissionItem(vehicle, flyView, parent);
+
+        int sequenceNumber = detail::nextSequenceNumber(list);
+        newItem->setSequenceNumber(sequenceNumber);
+        newItem->setCoordinate(coordinate);
+
+        // Set command (takeoff if first command).
+        newItem->setCommand(MAV_CMD_NAV_WAYPOINT);
+        if (list.count() == 1
+                && ( vehicle->fixedWing()
+                     || vehicle->vtol()
+                     || vehicle->multiRotor() ) ) {
+            MAV_CMD takeoffCmd = vehicle->vtol()
+                    ? MAV_CMD_NAV_VTOL_TAKEOFF
+                    : MAV_CMD_NAV_TAKEOFF;
+            if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
+                newItem->setCommand(takeoffCmd);
+            }
+        }
+
+        // Set altitude and altitude mode from previous item.
+        if (newItem->specifiesAltitude()) {
+            double  altitude;
+            int     altitudeMode;
+
+            if (detail::previousAltitude(list, --index, altitude, altitudeMode)) {
+                newItem->altitude()->setRawValue(altitude);
+                newItem->setAltitudeMode(
+                            static_cast<QGroundControlQmlGlobal::AltitudeMode>(
+                                altitudeMode) );
+            }
+        }
+
+        list.insert(index, newItem);
+
+        if ( doUpdates ) {
+            detail::updateSequenceNumbers(list, index);
+            detail::updateHirarchy(list);
+            detail::updateHomePosition(list);
+        }
+    }
+    return true;
+}
+
+size_t WaypointManager::Utils::detail::nextSequenceNumber(QmlObjectListModel &list)
+{
+    if (list.count() >= 1) {
+        VisualMissionItem* lastItem = list.value<VisualMissionItem*>(list.count() - 1);
+        return lastItem->lastSequenceNumber() + 1;
+    }
+    return 0;
+}
+
+bool WaypointManager::Utils::detail::previousAltitude(QmlObjectListModel &list,
+                                                      size_t index,
+                                                      double &altitude,
+                                                      int &altitudeMode)
+{
+    if (index >= size_t(list.count())) {
+        return false;
+    }
+
+    for (long i = index; i >= 0; --i) {
+        VisualMissionItem* visualItem = list.value<VisualMissionItem*>(i);
+
+        if ( visualItem->specifiesCoordinate()
+                 && !visualItem->isStandaloneCoordinate() ) {
+            if ( visualItem->isSimpleItem() ) {
+                SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
+                if (simpleItem->specifiesAltitude()) {
+                    altitude = simpleItem->altitude()->rawValue().toDouble();
+                    altitudeMode = simpleItem->altitudeMode();
+                    return true;
+                }
+            }
+        }
+    }
+
+    return false;
+}
+
+bool WaypointManager::Utils::detail::updateSequenceNumbers(QmlObjectListModel &list, size_t startIdx)
+{
+    using namespace WaypointManager::Utils;
+
+    if ( list.count() < 1 || startIdx >= size_t(list.count()) )
+        return false;
+
+    // Setup ascending sequence numbers for all visual items.
+    VisualMissionItem* startItem = list.value<VisualMissionItem*>(
+                std::max(long(startIdx)-1,long(0)) );
+    if (list.count() == 1){
+        startItem->setSequenceNumber(0);
+    } else {
+        int sequenceNumber = startItem->lastSequenceNumber() + 1;
+        for (int i=startIdx; i < list.count(); ++i) {
+            VisualMissionItem* item = list.value<VisualMissionItem*>(i);
+            item->setSequenceNumber(sequenceNumber);
+            sequenceNumber = item->lastSequenceNumber() + 1;
+        }
+    }
+    return true;
+}
+
+bool WaypointManager::Utils::detail::updateHirarchy(QmlObjectListModel &list)
+{
+    if ( list.count() < 1)
+        return false;
+
+    VisualMissionItem* currentParentItem = list.value<VisualMissionItem*>(0);
+
+    currentParentItem->childItems()->clear();
+
+    for (size_t i = 1; i < size_t( list.count() ); ++i) {
+        VisualMissionItem* item = list.value<VisualMissionItem*>(i);
+
+        if (item->specifiesCoordinate()) {
+            item->childItems()->clear();
+            currentParentItem = item;
+        } else if (item->isSimpleItem()) {
+            currentParentItem->childItems()->append(item);
+        }
+    }
+
+    return true;
+}
+
+bool WaypointManager::Utils::detail::updateHomePosition(QmlObjectListModel &list)
+{
+    MissionSettingsItem *settingsItem = list.value<MissionSettingsItem *>(0);
+
+    // Set the home position to be a delta from first coordinate
+    for (int i = 1; i < list.count(); ++i) {
+        VisualMissionItem* item = list.value<VisualMissionItem*>(i);
+
+        if (item->specifiesCoordinate() && item->coordinate().isValid()) {
+            QGeoCoordinate c = item->coordinate().atDistanceAndAzimuth(30, 0);
+            c.setAltitude(0);
+            settingsItem->setCoordinate(c);
+            return true;
+        }
+    }
+
+    return false;
+}
+
+void WaypointManager::Utils::initialize(QmlObjectListModel &list,
+                                        Vehicle *vehicle,
+                                        bool flyView)
+{
+    MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, flyView, &list);
+    list.insert(0, settingsItem);
+}
+
+bool WaypointManager::Utils::doUpdate(QmlObjectListModel &list)
+{
+    using namespace WaypointManager::Utils;
+
+    bool ret = detail::updateSequenceNumbers(list, 0);
+    ret = detail::updateHirarchy(list) == false ? false : ret;
+    (void)detail::updateHomePosition(list);
+
+    return ret;
+}

src/Wima/WaypointManager/utils.h → src/Wima/WaypointManager/Utils.h

 #pragma once
 
+#include "SimpleMissionItem.h"
+#include "QmlObjectListModel.h"
+
+#include <QVariant>
+
 namespace WaypointManager {
 namespace Utils {
 
@@ -10,7 +15,7 @@ CoordinateType getCoordinate(const SimpleMissionItem* item){
 }
 
 template<>
-QVariant getCoordinate<QVariant>(const SimpleMissionItem* item);
+QVariant getCoordinate(const SimpleMissionItem* item);
 
 
 /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
@@ -99,5 +104,76 @@ bool extract(ContainerType<CoordinateType>  &source,
     return true;
 }
 
-} // namespace WaypointManager
+
+//!
+//! \brief Initializes the list \p list.
+//!
+//! Adds a MissionSettingsItem to \p list.
+//! \param list List of VisualMissionItems.
+//! \param vehicle Vehicle.
+//! \param flyView Fly- or planview.
+void initialize(QmlObjectListModel &list,
+                Vehicle *vehicle,
+                bool flyView=true);
+
+//! @brief Creates (from QGeoCoordinate) and Inserts a SimpleMissionItem at the given index to list.
+//! \param coordinate Coordinate of the SimpleMissionItem.
+//! \param index Index to insert SimpleMissionItem.
+//! \param list List of SimpleMissionItems.
+//! \param vehicle Vehicle.
+//! \param flyView Fly- or planview.
+//! \param parent Parent of the SimpleMissionItem (Set to &list if nullptr).
+//! \param doUpdates Neccessary update are performed to the list \p list if set to true. Set this to true for the last item only,
+//! if inserting multiple items.
+//! \return Returns true on success, false either.
+//!
+//! @note The list must be initialized. (\see \fn initialize)
+//! @note This function doesn't establish any signal-slot connections (\see \fn MissionController::_initVisualItem).
+//! @note This function doesn't set the MissionFlightStatus_t (\see \fn VisualMissionItem::setMissionFlightStatus).
+bool insertMissionItem(const QGeoCoordinate &coordinate,
+                       long index,
+                       QmlObjectListModel &list,
+                       Vehicle *vehicle,
+                       bool flyView=true,
+                       QObject *parent=nullptr,
+                       bool doUpdates=true);
+
+bool doUpdate(QmlObjectListModel &list);
+
+namespace detail {
+    size_t nextSequenceNumber(QmlObjectListModel &list);
+    bool previousAltitude(QmlObjectListModel &list,
+                          size_t index,
+                          double &altitude,
+                          int &altitudeMode);
+
+    //! @brief Updates the sequence numbers of the VisualMissionItems inside \p list beginning from \p startIdx.
+    //!
+    //! Updates the sequence numbers of the VisualMissionItems inside \p list beginning from \pstartIdx.
+    //! \param list List containing VisualMissionItems.
+    //! \param startIdx Start index.
+    //! \return Returns true if successfull, false either.
+    //!
+    //! \note The function returns false immediatelly if the list \p list ist empty or
+    //! the \p startIdx is out of bounds.
+    //! \note If the list \p list contains only one VisualMissionItem it's sequence number is
+    //! set to 0.
+    bool updateSequenceNumbers(QmlObjectListModel &list, size_t startIdx=0);
+
+    //! @brief Update the parent child item hierarchy of the VisualMissionItems inside \p list.
+    //!
+    //! \param list List containing VisualMissionItems.
+    //! \return Returns true if successfull, false either.
+    //!
+    //! \note Returns true on success, false either.
+    bool updateHirarchy(QmlObjectListModel &list);
+
+    //! @brief Updates the home position to the first valid coordinate of the VisualMissionItems inside \p list.
+    //!
+    //! \param list List containing VisualMissionItems.
+    //! \return Returns true if the home position was updated, false either.
+    bool updateHomePosition(QmlObjectListModel &list);
+
+} // namespace detail
+} // namespace Utils
 } // namespace WaypointManager

src/Wima/WaypointManager/utils.cpp

-#include "utils.h"
-
-
-template<>
-QVariant getCoordinate<QVariant>(const SimpleMissionItem* item)
-{
-    return QVariant::fromValue(item->coordinate());
-}

src/Wima/WimaController.cc

@@ -6,9 +6,9 @@
 #include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
 #include "ros_bridge/include/CasePacker.h"
 
-#include "QtROSJsonFactory.h"
-#include "QtROSTypeFactory.h"
-#include "QNemoProgress.h"
+#include "Snake/QtROSJsonFactory.h"
+#include "Snake/QtROSTypeFactory.h"
+#include "Snake/QNemoProgress.h"
 
 #include "time.h"
 #include "assert.h"

src/Wima/WimaController.h

@@ -6,14 +6,14 @@
 #include "QGCMapPolygon.h"
 #include "QmlObjectListModel.h"
 
-#include "WimaArea.h"
-#include "WimaMeasurementArea.h"
-#include "WimaServiceArea.h"
-#include "WimaCorridor.h"
+#include "Geometry/WimaArea.h"
+#include "Geometry/WimaMeasurementArea.h"
+#include "Geometry/WimaServiceArea.h"
+#include "Geometry/WimaCorridor.h"
 #include "WimaDataContainer.h"
-#include "WimaMeasurementAreaData.h"
-#include "WimaCorridorData.h"
-#include "WimaServiceAreaData.h"
+#include "Geometry/WimaMeasurementAreaData.h"
+#include "Geometry/WimaCorridorData.h"
+#include "Geometry/WimaServiceAreaData.h"
 
 #include "PlanMasterController.h"
 #include "MissionController.h"
@@ -27,11 +27,11 @@
 #include "SettingsManager.h"
 
 #include "snake.h"
-#include "SnakeWorker.h"
-#include "SnakeTiles.h"
-#include "SnakeTilesLocal.h"
-#include "GeoPoint3D.h"
-#include "QNemoProgress.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"
 

src/Wima/WimaPlanData.h

@@ -2,11 +2,11 @@
 
 #include <QObject>
 
-#include "WimaAreaData.h"
-#include "WimaServiceAreaData.h"
-#include "WimaCorridorData.h"
-#include "WimaMeasurementAreaData.h"
-#include "WimaJoinedAreaData.h"
+#include "Geometry/WimaAreaData.h"
+#include "Geometry/WimaServiceAreaData.h"
+#include "Geometry/WimaCorridorData.h"
+#include "Geometry/WimaMeasurementAreaData.h"
+#include "Geometry/WimaJoinedAreaData.h"
 #include "MissionItem.h"
 
 class WimaPlanData : QObject

src/Wima/WimaPlaner.h

@@ -4,16 +4,16 @@
 #include "QGCMapPolygon.h"
 #include "QmlObjectListModel.h"
 
-#include "WimaArea.h"
-#include "WimaAreaData.h"
-#include "WimaServiceArea.h"
-#include "WimaServiceAreaData.h"
-#include "WimaMeasurementArea.h"
-#include "WimaMeasurementAreaData.h"
-#include "WimaCorridor.h"
-#include "WimaCorridorData.h"
-#include "WimaJoinedArea.h"
-#include "WimaJoinedAreaData.h"
+#include "Geometry/WimaArea.h"
+#include "Geometry/WimaAreaData.h"
+#include "Geometry/WimaServiceArea.h"
+#include "Geometry/WimaServiceAreaData.h"
+#include "Geometry/WimaMeasurementArea.h"
+#include "Geometry/WimaMeasurementAreaData.h"
+#include "Geometry/WimaCorridor.h"
+#include "Geometry/WimaCorridorData.h"
+#include "Geometry/WimaJoinedArea.h"
+#include "Geometry/WimaJoinedAreaData.h"
 #include "WimaPlanData.h"
 #include "WimaDataContainer.h"
 
@@ -26,8 +26,8 @@
 #include "QGCApplication.h"
 
 #include "OptimisationTools.h"
-#include "PlanimetryCalculus.h"
-#include "GeoUtilities.h"
+#include "Geometry/PlanimetryCalculus.h"
+#include "Geometry/GeoUtilities.h"
 
 #include "QSignalBlocker"