working on waypoint manager

qgroundcontrol.pro

@@ -438,10 +438,12 @@ HEADERS += \
     src/Wima/Snake/SnakeTiles.h \
     src/Wima/Snake/SnakeTilesLocal.h \
     src/Wima/Snake/SnakeWorker.h \
-    src/Wima/WaypointManager/GenericPathSlicer.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/api/QGCCorePlugin.h \
     src/api/QGCOptions.h \
     src/api/QGCSettings.h \
@@ -495,8 +497,10 @@ SOURCES += \
     src/Wima/Geometry/PolygonArray.cc \
     src/Wima/Snake/QNemoProgress.cc \
     src/Wima/Snake/SnakeWorker.cc \
-    src/Wima/WaypointManager/GenericPathSlicer.cpp \
+    src/Wima/WaypointManager/GenericSlicer.cpp \
     src/Wima/WaypointManager/GenericWaypointManager.cpp \
+    src/Wima/WaypointManager/Settings.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/WaypointManager/GenericPathSlicer.cpp

-#include "GenericPathSlicer.h"

src/Wima/WaypointManager/GenericSlicer.cpp

+#include "GenericSlicer.h"

src/Wima/WaypointManager/GenericPathSlicer.h → src/Wima/WaypointManager/GenericSlicer.h

@@ -3,31 +3,22 @@
 #include <assert.h>
 #include <iostream>
 
+#include "utils.h"
+
 //! @brief Base class for all waypoint managers.
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-class GenericPathSlicer
+class GenericSlicer
 {
 public:
     typedef Container<ElementType> ContainerType;
-    GenericPathSlicer();
-
-    const ContainerType &path  () const;
-
-    // Waypoint editing.
-    void            setPath     (const ContainerType &path);
-    void            push_back   (const ElementType &wp);
-    void            push_front  (const ElementType &wp);
-    void            clear       ();
-    void            insert      (int i, const ElementType &wp);
-    uint32_t size();
-    ElementType     &at         (unsigned int i);
+    GenericSlicer();
 
 
     // Slicing.
-    void slice      (ContainerType &slice);
-    void next       (ContainerType &slice);
-    void previous   (ContainerType &slice);
-    void reset      (ContainerType &slice);
+    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);
 
 
     // Slicing parameters.
@@ -51,16 +42,11 @@ public:
     uint32_t      N             ();
     //! @return Returns the start index.
     int           startIndex    ();
-    //! @return Returns the end index.
-    int           endIndex    ();
-    //! @return Returns the start index of the next slice.
-    int           nextIndex    ();
 
 
 private:
-    void _updateIdx();
+    void _updateIdx(std::size_t size);
 
-    ContainerType _path;
     long          _idxStart;
     long          _idxEnd;
     long          _idxNext;
@@ -75,122 +61,56 @@ private:
 
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-GenericPathSlicer<ElementType, Container>::GenericPathSlicer():
+GenericSlicer<ElementType, Container>::GenericSlicer():
     _idxValid(false)
   , _atEnd(false)
 {}
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-const Container<ElementType> &GenericPathSlicer<ElementType, Container>::path() const {
-    return _path;
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::setPath(const ContainerType &waypoints) {
-    _idxValid = false;
-    _path = waypoints;
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::push_back(const ElementType &wp) {
-    _idxValid = false;
-    _path.push_back(wp);
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::push_front(const ElementType &wp) {
-    _idxValid = false;
-    _path.push_front(wp);
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::clear(){
-    _idxValid = false;
-    _path.clear();
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::insert(int i, const ElementType &wp){
-    _idxValid = false;
-    _path.insert(i, wp);
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-std::uint32_t GenericPathSlicer<ElementType, Container>::size()
-{
-    return std::uint32_t(_path.size());
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-ElementType &GenericPathSlicer<ElementType, Container>::at(unsigned int i)
-{
-    return _path.at(i);
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::setOverlap(std::uint32_t overlap)
+void GenericSlicer<ElementType, Container>::setOverlap(std::uint32_t overlap)
 {
     _idxValid = false;
     _overlap = overlap;
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::setN(uint32_t N)
+void GenericSlicer<ElementType, Container>::setN(uint32_t N)
 {
     _idxValid = false;
     _N = N > 0 ? N : 1;
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::setStartIndex(int idxStart)
+void GenericSlicer<ElementType, Container>::setStartIndex(int idxStart)
 {
     _idxValid = false;
     _idxStart = idxStart;
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-uint32_t GenericPathSlicer<ElementType, Container>::overlap()
+uint32_t GenericSlicer<ElementType, Container>::overlap()
 {
     return _overlap;
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-uint32_t GenericPathSlicer<ElementType, Container>::N()
+uint32_t GenericSlicer<ElementType, Container>::N()
 {
     return _N;
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-int GenericPathSlicer<ElementType, Container>::startIndex()
+int GenericSlicer<ElementType, Container>::startIndex()
 {
-    if (!_idxValid)
-        _updateIdx();
     return _idxStart;
 }
 
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-int GenericPathSlicer<ElementType, Container>::endIndex()
-{
-    if (!_idxValid)
-        _updateIdx();
-    return _idxEnd;
-}
-
-template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-int GenericPathSlicer<ElementType, Container>::nextIndex()
-{
-    if (!_idxValid)
-        _updateIdx();
-    return _idxNext;
-}
-
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::_updateIdx()
+void GenericSlicer<ElementType, Container>::_updateIdx(std::size_t size)
 {
     _idxValid = true;
     _atEnd    = false;
-    std::uint64_t size = _path.size();
     if ( _idxStart >= size-1 ) {
         _idxStart   = size-1;
         _idxEnd     = _idxStart;
@@ -209,37 +129,36 @@ void GenericPathSlicer<ElementType, Container>::_updateIdx()
     _idxNext = _idxNext < size ? _idxNext : size-1;
 
     _idxPrevious = _idxStart - 1 + _overlap;
-    _idxPrevious = _idxPrevious < 0    ? 0        : _idxPrevious;
+    _idxPrevious = _idxPrevious < 0    ? 0            : _idxPrevious;
     _idxPrevious = _idxPrevious < size ? _idxPrevious : size-1;
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::slice(Container<ElementType> &c){
+void GenericSlicer<ElementType, Container>::slice(const ContainerType &source,
+                                                  Container<ElementType> &slice){
 
     if ( !_idxValid)
-        _updateIdx();
-
-    (void)c;
-    assert(false); //ToDo
+        _updateIdx(source.size());
 
+    WaypointManager::Utils::extract(source, slice, _idxStart, _idxEnd);
     // extract waypoints
 
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::next(Container<ElementType> &c){
+void GenericSlicer<ElementType, Container>::next(const ContainerType &source, Container<ElementType> &slice){
     setStartIndex(_idxNext);
-    slice(c);
+    slice(source, slice);
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::previous(Container<ElementType> &c){
+void GenericSlicer<ElementType, Container>::previous(const ContainerType &source, Container<ElementType> &slice){
     setStartIndex(_idxPrevious);
-    slice(c);
+    slice(source, slice);
 }
 
 template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
-void GenericPathSlicer<ElementType, Container>::reset(Container<ElementType> &c){
+void GenericSlicer<ElementType, Container>::reset(const ContainerType &source, Container<ElementType> &slice){
     setStartIndex(0);
-    slice(c);
+    slice(source, slice);
 }

src/Wima/WaypointManager/GenericWaypointManager.cpp

 #include "GenericWaypointManager.h"
-
-GenericWaypointManager::GenericWaypointManager()
-{
-
-}

src/Wima/WaypointManager/GenericWaypointManager.h

 #pragma once
 
+#include <iostream>
 
+#include "GenericSlicer.h"
+#include "Settings.h"
+
+namespace WaypointManager {
+
+template<class WaypointType,
+         template<class, class...> class ContainerType>
 class GenericWaypointManager
 {
+    typedef GenericSlicer<WaypointType, ContainerType> Slicer;
 public:
-    GenericWaypointManager();
+    typedef ContainerType<WaypointType>  WaypointList;
+
+    GenericWaypointManager() = delete;
+    GenericWaypointManager(Slicer *slicer, Settings *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);
+
+
+    const ContainerType &getWaypoints  () ;
+    const ContainerType &getSlcieaypoints  () ;
+    const ContainerType &missionItems  () ;
+
+    void slice();
+    void next();
+    void previous();
+    void reset();
+
+private:
+    WaypointList _waypoints;
+    WaypointList _slice;
+    bool         _sliceValid;
+    Slicer      *_slicer;
+    Settings    *_settings;
+
 };
+
+
+template<class WaypointType,
+         template<class, class...> class ContainerType>
+GenericWaypointManager<WaypointType, ContainerType>::GenericWaypointManager(
+        Slicer *slicer,
+        Settings *Settings) :
+    _sliceValid(false)
+  , _slicer(slicer)
+  , _settings(_settings)
+{}
+
+
+
+} // namespace WaypointManager
+

src/Wima/WaypointManager/Settings.cpp

+#include "Settings.h"

src/Wima/WaypointManager/Settings.h

+#pragma once
+
+namespace WaypointManager {
+
+class Settings
+{
+public:
+    Settings();
+};
+
+} // namespace WaypointManager

src/Wima/WaypointManager/utils.cpp

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

src/Wima/WaypointManager/utils.h

+#pragma once
+
+namespace WaypointManager {
+namespace Utils {
+
+
+template<class CoordinateType>
+CoordinateType getCoordinate(const SimpleMissionItem* item){
+    return item->coordinate();
+}
+
+template<>
+QVariant getCoordinate<QVariant>(const SimpleMissionItem* item);
+
+
+/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
+template <class CoordinateType, template <class, class...> class ContainerType>
+bool extract(QmlObjectListModel &missionItems,
+             ContainerType<CoordinateType> &coordinateList,
+             std::size_t startIndex,
+             std::size_t  endIndex)
+{
+
+    if (   startIndex < std::size_t(missionItems.count())
+        && endIndex   < std::size_t(missionItems.count())
+        && missionItems.count() > 0) {
+        if (startIndex > endIndex) {
+            if ( !extract(missionItems,
+                          coordinateList,
+                          startIndex,
+                          std::size_t(missionItems.count()-1)) /*recursion*/)
+                return false;
+            if ( !extract(missionItems,
+                          coordinateList,
+                          0,
+                          endIndex) /*recursion*/)
+                return false;
+        } else {
+            for (std::size_t i = startIndex; i <= endIndex; ++i) {
+                SimpleMissionItem *mItem = missionItems.value<SimpleMissionItem *>(int(i));
+
+                if (mItem == nullptr) {
+                    coordinateList.clear();
+                    return false;
+                }
+                coordinateList.append(getCoordinate<CoordinateType>(mItem));
+            }
+        }
+    } else
+        return false;
+
+    return true;
+}
+
+/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
+template <class CoordinateType, template <class, class...> class ContainerType>
+bool extract(QmlObjectListModel &missionItems,
+             ContainerType<CoordinateType> &coordinateList)
+{
+
+    return extract(missionItems,
+                   coordinateList,
+                   std::size_t(0),
+                   std::size_t(missionItems.count())
+                   );
+}
+
+
+/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
+template <class CoordinateType, template <class, class...> class ContainerType>
+bool extract(ContainerType<CoordinateType>  &source,
+             ContainerType<CoordinateType>  &destination,
+             std::size_t startIndex,
+             std::size_t  endIndex)
+{
+
+    if (   startIndex < std::size_t(source.size())
+        && endIndex   < std::size_t(source.size())
+        && source.size() > 0) {
+        if (startIndex > endIndex) {
+            if ( !extract(source,
+                          destination,
+                          startIndex,
+                          std::size_t(source.size()-1)) /*recursion*/)
+                return false;
+            if ( !extract(source,
+                          destination,
+                          0,
+                          endIndex) /*recursion*/)
+                return false;
+        } else {
+            for (std::size_t i = startIndex; i <= endIndex; ++i) {
+                destination.append(source[i]);
+            }
+        }
+    } else
+        return false;
+
+    return true;
+}
+
+} // namespace WaypointManager
+} // namespace WaypointManager

src/Wima/WimaController.cc

@@ -111,6 +111,18 @@ WimaController::WimaController(QObject *parent)
     _startStopRosBridge();
 }
 
+PlanMasterController *WimaController::masterController() {
+    return _masterController;
+}
+
+MissionController *WimaController::missionController() {
+    return _missionController;
+}
+
+QmlObjectListModel *WimaController::visualItems() {
+    return &_visualItems;
+}
+
 QVariantList WimaController::waypointPath() const
 {
     QVariantList list;