Skip to content

Q
qgroundcontrol
Commit 51adc15a authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

working on waypoint manager

parent 6a082ad8
Hide whitespace changes
Inline Side-by-side
Showing with 500 additions and 45 deletions
qgroundcontrol.pro
View file @ 51adc15a
......@@ -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.h → src/Wima/WaypointManager/GenericSlicer.h
View file @ 51adc15a
......@@ -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
View file @ 51adc15a
#include "GenericWaypointManager.h"
GenericWaypointManager::GenericWaypointManager()
{
}
src/Wima/WaypointManager/GenericWaypointManager.h
View file @ 51adc15a
#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.h 0 → 100644
View file @ 51adc15a
#pragma once
namespace WaypointManager {
class Settings
{
public:
Settings();
};
} // namespace WaypointManager
src/Wima/WaypointManager/utils.cpp 0 → 100644
View file @ 51adc15a
#include "utils.h"
template<>
QVariant getCoordinate<QVariant>(const SimpleMissionItem* item)
{
return QVariant::fromValue(item->coordinate());
}
src/Wima/WaypointManager/utils.h 0 → 100644
View file @ 51adc15a
#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
View file @ 51adc15a
......@@ -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;
......
src/Wima/WimaController.h
View file @ 51adc15a
......@@ -62,52 +62,154 @@ public:
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(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)
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<int> nemoProgress READ nemoProgress NOTIFY nemoProgressChanged)
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<int> nemoProgress
READ nemoProgress
NOTIFY nemoProgressChanged
)
// Property accessors
PlanMasterController* masterController (void) { return _masterController; }
MissionController* missionController (void) { return _missionController; }
QmlObjectListModel* visualItems (void) { return &_visualItems; }
PlanMasterController* masterController (void);
MissionController* missionController (void);
QmlObjectListModel* visualItems (void);
// QString currentFile (void) const { return _currentFile; }
// QStringList loadNameFilters (void) const;
// QStringList saveNameFilters (void) const;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment