Commit d02c3c46 authored by Valentin Platzgummer's avatar Valentin Platzgummer

refactoring WimaController, adding waypoint managers.

parent 8b365d4f
......@@ -437,7 +437,9 @@ HEADERS += \
src/Wima/QtROSTypeFactory.h \
src/Wima/SnakeTiles.h \
src/Wima/SnakeTilesLocal.h \
src/Wima/WimaControllerDetail.h \
src/Wima/SnakeWorker.h \
src/Wima/WaypointManager/GenericPathSlicer.h \
src/Wima/WaypointManager/GenericWaypointManager.h \
src/Wima/WimaPolygonArray.h \
src/Wima/snaketile.h \
src/api/QGCCorePlugin.h \
......@@ -493,12 +495,14 @@ SOURCES += \
src/Wima/GeoPoint3D.cpp \
src/Wima/PolygonArray.cc \
src/Wima/QNemoProgress.cc \
src/Wima/SnakeWorker.cc \
src/Wima/WaypointManager/GenericPathSlicer.cpp \
src/Wima/WaypointManager/GenericWaypointManager.cpp \
src/comm/ros_bridge/include/ComPrivateInclude.cpp \
src/comm/ros_bridge/include/MessageTag.cpp \
src/comm/ros_bridge/include/TopicPublisher.cpp \
src/comm/ros_bridge/include/TopicSubscriber.cpp \
src/comm/ros_bridge/src/CasePacker.cpp \
src/Wima/WimaControllerDetail.cc \
src/Wima/snaketile.cpp \
src/api/QGCCorePlugin.cc \
src/api/QGCOptions.cc \
......
......@@ -244,9 +244,12 @@ FlightMap {
// Add Snake tiles center points to the map
MapItemView {
property bool _lengthMatching: wimaController.snakeTileCenterPoints.length
=== wimaController.snakeProgress.length
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
&& _lengthMatching
model: _enable ? wimaController.snakeTileCenterPoints : 0
......
......@@ -378,7 +378,7 @@ int MissionController::_nextSequenceNumber(void)
}
}
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
int MissionController::insertSimpleMissionItem(const QGeoCoordinate &coordinate, int i)
{
int sequenceNumber = _nextSequenceNumber();
SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
......@@ -410,6 +410,58 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
return newItem->sequenceNumber();
}
int MissionController::insertSimpleMissionItems(const QList<QGeoCoordinate> &coordinates, int idx)
{
MAV_CMD mavCmd = MAV_CMD_NAV_WAYPOINT;
if ( _visualItems->count() == 1
&& ( _controllerVehicle->fixedWing()
|| _controllerVehicle->vtol()
|| _controllerVehicle->multiRotor()) )
{
mavCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF;
if ( !_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(mavCmd)) {
mavCmd = MAV_CMD_NAV_WAYPOINT;
}
}
int sequenceNumber = _nextSequenceNumber();
int size = coordinates.size();
SimpleMissionItem *newItem = nullptr;
bool firstItem = true;
double prevAltitude = 0.0;
int prevAltitudeMode = 0;
for (int i = 0; i < size; ++i) {
newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
newItem->setSequenceNumber(sequenceNumber);
newItem->setCoordinate(coordinates[i]);
newItem->setCommand(mavCmd);
_initSimpleItem(newItem);
if (!firstItem || ( newItem->specifiesAltitude()
&& _findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) )
{
newItem->altitude()->setRawValue(prevAltitude);
newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
}
newItem->setMissionFlightStatus(_missionFlightStatus);
_visualItems->insert(idx++, newItem);
sequenceNumber = newItem->lastSequenceNumber() + 1;
mavCmd = MAV_CMD_NAV_WAYPOINT;
firstItem = false;
}
_recalcSequence();
_recalcChildItems();
_recalcWaypointLines();
_updateTimer.start(UPDATE_TIMEOUT);
if ( newItem == nullptr)
return -1;
return newItem->sequenceNumber();
}
int MissionController::insertSimpleMissionItem(const MissionItem &missionItem, int i)
{
int sequenceNumber = _nextSequenceNumber();
......@@ -1674,7 +1726,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoo
}
/// @param clickCoordinate The location of the user click when inserting a new item
void MissionController::_recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate)
void MissionController::_recalcAllWithClickCoordinate(const QGeoCoordinate& clickCoordinate)
{
if (!_flyView) {
_setPlannedHomePositionFromFirstCoordinate(clickCoordinate);
......@@ -1770,37 +1822,61 @@ void MissionController::_deinitAllVisualItems(void)
void MissionController::_initVisualItem(VisualMissionItem* visualItem)
{
setDirty(false);
connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged, this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::terrainAltitudeChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence);
if (visualItem->isSimpleItem()) {
// We need to track commandChanged on simple item since recalc has special handling for takeoff command
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
if (simpleItem) {
connect(&simpleItem->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
_initSimpleItem(simpleItem);
} else {
qWarning() << "isSimpleItem == true, yet not SimpleMissionItem";
}
} else {
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
if (complexItem) {
connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(complexItem, &ComplexMissionItem::greatestDistanceToChanged, this, &MissionController::_recalcMissionFlightStatus);
_initComplexItem(complexItem);
} else {
qWarning() << "ComplexMissionItem not found";
}
}
}
void MissionController::_initVisualItemCommon(VisualMissionItem *visualItem)
{
setDirty(false);
connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,
this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,
this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,
this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::terrainAltitudeChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged,
this, &MissionController::_recalcSequence);
}
void MissionController::_initSimpleItem(SimpleMissionItem *item)
{
_initVisualItemCommon(item);
// We need to track commandChanged on simple item since recalc has special handling for takeoff command
connect(&item->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
}
void MissionController::_initComplexItem(ComplexMissionItem *item)
{
_initVisualItemCommon(item);
connect(item, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(item, &ComplexMissionItem::greatestDistanceToChanged, this, &MissionController::_recalcMissionFlightStatus);
}
void MissionController::_deinitVisualItem(VisualMissionItem* visualItem)
{
// Disconnect all signals
......
......@@ -109,7 +109,12 @@ public:
/// Add a new simple mission item to the list
/// @param i: index to insert at
/// @return Sequence number for new item
Q_INVOKABLE int insertSimpleMissionItem(QGeoCoordinate coordinate, int i);
Q_INVOKABLE int insertSimpleMissionItem(const QGeoCoordinate &coordinate, int i);
/// Add a list of new simple mission item to the list.
/// @param i: index to insert at
/// @return Sequence number of last item.
Q_INVOKABLE int insertSimpleMissionItems(const QList<QGeoCoordinate> &coordinates, int i);
/// Add a new simple mission item to the list
/// @param i: index to insert at
......@@ -289,10 +294,13 @@ private:
void _init(void);
void _recalcSequence(void);
void _recalcChildItems(void);
void _recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate);
void _recalcAllWithClickCoordinate(const QGeoCoordinate &clickCoordinate);
void _initAllVisualItems(void);
void _deinitAllVisualItems(void);
void _initVisualItem(VisualMissionItem* item);
void _initVisualItemCommon(VisualMissionItem* visualItem); // Don't call this function, it is used by _initSimpleItem and _initComplexItem.
void _initSimpleItem(SimpleMissionItem* item);
void _initComplexItem(ComplexMissionItem* item);
void _deinitVisualItem(VisualMissionItem* item);
void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
......
......@@ -274,7 +274,8 @@ void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, dou
bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy);
polygonOffset = result[0];
if (result.size() > 0)
polygonOffset = result[0];
}
......
#include "QNemoProgress.h"
QNemoProgress::QNemoProgress(QObject *parent) :
ProgressBase()
, QObject(parent)
{
}
QNemoProgress::QNemoProgress(const QNemoProgress &other, QObject *parent):
ProgressBase(other)
, QObject(parent)
{
}
QNemoProgress *QNemoProgress::Clone() const {
return new QNemoProgress(*this, this->parent());
}
QVector<int> &QNemoProgress::progress() {
emit QNemoProgress::progressChanged();
return _progress;
}
const QVector<int> &QNemoProgress::progress() const {
return _progress;
}
......@@ -5,22 +5,6 @@
#include "ros_bridge/include/GenericMessages.h"
namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs;
typedef NemoMsgs::GenericProgress<int, QVector> ProgressBase;
class QNemoProgress : public ProgressBase, public QObject {
public:
QNemoProgress(QObject *parent = nullptr);
QNemoProgress(const QNemoProgress &other, QObject *parent = nullptr);
virtual QNemoProgress *Clone() const override;
virtual const QVector<int> &progress(void) const override;
virtual QVector<int> &progress(void) override;
signals:
void progressChanged();
};
typedef NemoMsgs::GenericProgress<int, QVector> QNemoProgress;
#include "WimaControllerDetail.h"
#include "SnakeWorker.h"
#include <QGeoCoordinate>
......@@ -6,7 +6,10 @@ SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
, _lineDistance (3) // meter
, _minTransectLength (2) // meter
{
}
SnakeWorker::~SnakeWorker()
{
}
void SnakeWorker::setScenario(const Scenario &scenario)
......@@ -80,6 +83,4 @@ void SnakeWorker::run()
}
}
emit resultReady();
}
......@@ -28,6 +28,7 @@ class SnakeWorker : public QThread{
public:
SnakeWorker(QObject *parent = nullptr);
~SnakeWorker() override;
void setScenario (const Scenario &scenario);
void setProgress (const QVector<int> &progress);
......@@ -36,9 +37,6 @@ public:
const WorkerResult_t &getResult() const;
signals:
void resultReady();
protected:
void run() override;
......
#pragma once
#include <assert.h>
#include <iostream>
//! @brief Base class for all waypoint managers.
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
class GenericPathSlicer
{
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);
// Slicing.
void slice (ContainerType &slice);
void next (ContainerType &slice);
void previous (ContainerType &slice);
void reset (ContainerType &slice);
// Slicing parameters.
//! @brief Sets the overlap.
//!
//! @param overlap The number of overlapping vertices
//! between on and the next slice.
void setOverlap (uint32_t overlap);
//! @brief Sets the number of vertices per slice.
//!
//! @param N The number of vertices per slice.
void setN (std::uint32_t N);
//! @brief Sets the start index.
//!
//! @param idxStart The start index.
void setStartIndex (int idxStart);
//! @return Returns the overlap.
uint32_t overlap ();
//! @return Returns the number of vertices per slice N.
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();
ContainerType _path;
long _idxStart;
long _idxEnd;
long _idxNext;
long _idxPrevious;
uint32_t _overlap;
uint32_t _N;
bool _idxValid;
bool _atEnd;
};
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
GenericPathSlicer<ElementType, Container>::GenericPathSlicer():
_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)
{
_idxValid = false;
_overlap = overlap;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<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)
{
_idxValid = false;
_idxStart = idxStart;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
uint32_t GenericPathSlicer<ElementType, Container>::overlap()
{
return _overlap;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
uint32_t GenericPathSlicer<ElementType, Container>::N()
{
return _N;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
int GenericPathSlicer<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()
{
_idxValid = true;
_atEnd = false;
std::uint64_t size = _path.size();
if ( _idxStart >= size-1 ) {
_idxStart = size-1;
_idxEnd = _idxStart;
_idxNext = _idxStart;
_atEnd = true;
return;
}
_idxStart < 0 ? 0 : _idxStart;
_idxEnd = _idxStart + _N - 1;
_idxEnd = _idxEnd < size ? _idxEnd : size-1;
_idxNext = _idxEnd + 1 - _overlap;
_idxNext = _idxNext < 0 ? 0 : _idxNext;
_idxNext = _idxNext < size ? _idxNext : size-1;
_idxPrevious = _idxStart - 1 + _overlap;
_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){
if ( !_idxValid)
_updateIdx();
(void)c;
assert(false); //ToDo
// extract waypoints
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::next(Container<ElementType> &c){
setStartIndex(_idxNext);
slice(c);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::previous(Container<ElementType> &c){
setStartIndex(_idxPrevious);
slice(c);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::reset(Container<ElementType> &c){
setStartIndex(0);
slice(c);
}
#include "GenericWaypointManager.h"
GenericWaypointManager::GenericWaypointManager()
{
}
#pragma once
class GenericWaypointManager
{
public:
GenericWaypointManager();
};
......@@ -17,7 +17,7 @@
#include <QScopedPointer>
const char* WimaController::wimaFileExtension = "wima";
// const char* WimaController::wimaFileExtension = "wima";
const char* WimaController::areaItemsName = "AreaItems";
const char* WimaController::missionItemsName = "MissionItems";
const char* WimaController::settingsGroup = "WimaController";
......@@ -101,29 +101,32 @@ WimaController::WimaController(QObject *parent)
_enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());
// Snake Worker Thread.
connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeStoreWorkerResults);
connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
// Start, stop RosBridge.
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
_startStopRosBridge();
}
QStringList WimaController::loadNameFilters() const
{
QStringList filters;
//QStringList WimaController::loadNameFilters() const
//{
// QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
tr("All Files (*.*)");
return filters;
}
// filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
// tr("All Files (*.*)");
// return filters;
//}
QStringList WimaController::saveNameFilters() const
{
QStringList filters;
//QStringList WimaController::saveNameFilters() const
//{
// QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
return filters;
}
// filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
// return filters;
//}
bool WimaController::uploadOverrideRequired() const
{
......@@ -331,37 +334,32 @@ void WimaController::removeVehicleTrajectoryHistory()
managerVehicle->trajectoryPoints()->clear();
}
void WimaController::saveToCurrent()
{
//void WimaController::saveToFile(const QString& filename)
//{
// QString file = filename;
//}
}
//bool WimaController::loadFromCurrent()
//{
// return true;
//}
void WimaController::saveToFile(const QString& filename)
{
QString file = filename;
}
//bool WimaController::loadFromFile(const QString &filename)
//{
// QString file = filename;
// return true;
//}
bool WimaController::loadFromCurrent()
{
return true;
}
bool WimaController::loadFromFile(const QString &filename)
{
QString file = filename;
return true;
}
//QJsonDocument WimaController::saveToJson(FileType fileType)
//{
// if(fileType)
// {
QJsonDocument WimaController::saveToJson(FileType fileType)
{
if(fileType)
{
}
return QJsonDocument();
}
// }
// return QJsonDocument();
//}