Commit 7427e2f7 authored by Valentin Platzgummer's avatar Valentin Platzgummer

Waypoint managers removed, code compiles

parent 3dc41359
......@@ -486,16 +486,9 @@ HEADERS += \
src/Wima/Snake/SnakeTileLocal.h \
src/Wima/Snake/SnakeTiles.h \
src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/StateMachine.h \
src/Wima/WaypointManager/AreaInterface.h \
src/Wima/WaypointManager/DefaultManager.h \
src/Wima/WaypointManager/EmptyManager.h \
src/Wima/WaypointManager/GenericWaypointManager.h \
src/Wima/WaypointManager/RTLManager.h \
src/Wima/WaypointManager/Settings.h \
src/Wima/WaypointManager/Slicer.h \
src/Wima/WaypointManager/Utils.h \
src/Wima/Utils.h \
src/Wima/WimaBridge.h \
src/Wima/WimaStateMachine.h \
src/Wima/call_once.h \
src/api/QGCCorePlugin.h \
src/api/QGCOptions.h \
......@@ -547,6 +540,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) {
SOURCES += \
src/Vehicle/VehicleEscStatusFactGroup.cc \
src/Wima/WimaStateMachine.cpp \
src/api/QGCCorePlugin.cc \
src/api/QGCOptions.cc \
src/api/QGCSettings.cc \
......@@ -563,15 +557,7 @@ SOURCES += \
src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \
src/Wima/Snake/SnakeTile.cpp \
src/Wima/StateMachine.cpp \
src/Wima/WaypointManager/AreaInterface.cpp \
src/Wima/WaypointManager/DefaultManager.cpp \
src/Wima/WaypointManager/EmptyManager.cpp \
src/Wima/WaypointManager/GenericWaypointManager.cpp \
src/Wima/WaypointManager/RTLManager.cpp \
src/Wima/WaypointManager/Settings.cpp \
src/Wima/WaypointManager/Slicer.cpp \
src/Wima/WaypointManager/Utils.cpp \
src/Wima/Utils.cpp \
src/Wima/WimaBridge.cc \
src/comm/ros_bridge/include/RosBridgeClient.cpp \
src/comm/ros_bridge/include/com_private.cpp \
......
......@@ -51,18 +51,6 @@ void WimaCorridor::init()
this->setObjectName(WimaCorridorName);
}
void print(const WimaCorridor &area)
{
QString message;
print(area, message);
qWarning() << message;
}
void print(const WimaCorridor &area, QString &outputString)
{
print(static_cast<const WimaArea&>(area), outputString);
}
/*!
\class WimaCorridor
......
......@@ -22,10 +22,6 @@ public:
// static Members
static const char *WimaCorridorName;
// Friends
friend void print(const WimaCorridor &area, QString &outputString);
friend void print(const WimaCorridor &area);
signals:
public slots:
......
......@@ -50,19 +50,6 @@ void WimaJoinedArea::init()
this->setObjectName(WimaJoinedAreaName);
}
void print(const WimaJoinedArea &area)
{
QString message;
print(area, message);
qWarning() << message;
}
void print(const WimaJoinedArea &area, QString &outputString)
{
print(static_cast<const WimaArea&>(area), outputString);
}
/*!
\class WimaJoinedArea
\brief Joined area (derived from \c WimaArea) composed by the \c WimaMeasurementArea, the \c WimaCorridor, and the \c WimaServiceArea.
......
......@@ -25,10 +25,6 @@ public:
// static Members
static const char* WimaJoinedAreaName;
// Friends
friend void print(const WimaJoinedArea& area, QString& outputString);
friend void print(const WimaJoinedArea& area);
signals:
public slots:
......
......@@ -85,18 +85,6 @@ bool WimaServiceArea::loadFromJson(const QJsonObject &json,
return retVal;
}
void print(const WimaServiceArea &area) {
QString message;
print(area, message);
qWarning() << message;
}
void print(const WimaServiceArea &area, QString &outputStr) {
print(static_cast<const WimaArea &>(area), outputStr);
outputStr.append(QString("Depot Position: %s\n")
.arg(area._depot.toString(QGeoCoordinate::Degrees)));
}
void WimaServiceArea::init() {
this->setObjectName(wimaServiceAreaName);
connect(this, &WimaArea::pathChanged, [this] {
......
......@@ -26,10 +26,6 @@ public:
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// Friends
friend void print(const WimaServiceArea &area, QString &outputStr);
friend void print(const WimaServiceArea &area);
// static Members
static const char *wimaServiceAreaName;
static const char *depotLatitudeName;
......
#include "AreaInterface.h"
WaypointManager::AreaInterface::AreaInterface()
: _mArea(nullptr)
, _sArea(nullptr)
, _cArea(nullptr)
, _jArea(nullptr)
{
}
WaypointManager::AreaInterface::AreaInterface(WimaMeasurementAreaData *mArea,
WimaServiceAreaData *sArea,
WimaCorridorData *corr,
WimaJoinedAreaData *jArea)
: _mArea(mArea)
, _sArea(sArea)
, _cArea(corr)
, _jArea(jArea)
{
}
void WaypointManager::AreaInterface::setMeasurementArea(WimaMeasurementAreaData *area)
{
_mArea = area;
}
void WaypointManager::AreaInterface::setServiceArea(WimaServiceAreaData *area)
{
_sArea = area;
}
void WaypointManager::AreaInterface::setCorridor(WimaCorridorData *area)
{
_cArea = area;
}
void WaypointManager::AreaInterface::setJoinedArea(WimaJoinedAreaData *area)
{
_jArea = area;
}
const WimaMeasurementAreaData *WaypointManager::AreaInterface::measurementArea() const
{
return _mArea;
}
const WimaServiceAreaData *WaypointManager::AreaInterface::serviceArea() const
{
return _sArea;
}
const WimaCorridorData *WaypointManager::AreaInterface::corridor() const
{
return _cArea;
}
const WimaJoinedAreaData *WaypointManager::AreaInterface::joinedArea() const
{
return _jArea;
}
WimaMeasurementAreaData *WaypointManager::AreaInterface::measurementArea()
{
return _mArea;
}
WimaServiceAreaData *WaypointManager::AreaInterface::serviceArea()
{
return _sArea;
}
WimaCorridorData *WaypointManager::AreaInterface::corridor()
{
return _cArea;
}
WimaJoinedAreaData *WaypointManager::AreaInterface::joinedArea()
{
return _jArea;
}
#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
#include "DefaultManager.h"
#include "Wima/Geometry/GeoUtilities.h"
#include "Wima/Geometry/PolygonCalculus.h"
#include "MissionSettingsItem.h"
#include "SimpleMissionItem.h"
WaypointManager::DefaultManager::DefaultManager(Settings &settings,
AreaInterface &interface)
: ManagerBase(settings), _areaInterface(&interface) {}
void WaypointManager::DefaultManager::clear() {
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
}
bool WaypointManager::DefaultManager::update() {
// extract waypoints
_currentWaypoints.clear();
Slicer::update(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::next() {
// extract waypoints
_currentWaypoints.clear();
Slicer::next(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::previous() {
// extract waypoints
_currentWaypoints.clear();
Slicer::previous(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::reset() {
// extract waypoints
_currentWaypoints.clear();
Slicer::reset(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::_insertMissionItem(
const QGeoCoordinate &c, size_t index, QmlObjectListModel &list,
bool doUpdate) {
using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->masterController(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) {
qWarning(
"WaypointManager::DefaultManager::next(): insertMissionItem failed.");
Q_ASSERT(false);
return false;
}
return true;
}
bool WaypointManager::DefaultManager::_insertMissionItem(
const QGeoCoordinate &c, size_t index, bool doUpdate) {
return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}
bool WaypointManager::DefaultManager::_calcShortestPath(
const QGeoCoordinate &start, const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path) {
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF joinedArea2D;
toCartesianList(_areaInterface->joinedArea()->coordinateList(),
/*origin*/ start, joinedArea2D);
QPointF start2D(0, 0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2DOut;
bool retVal =
PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
toGeoList(path2DOut, /*origin*/ start, path);
return retVal;
}
bool WaypointManager::DefaultManager::_worker() {
// Precondition:
// _waypoints must contain valid coordinates.
// Slicer must be called befor invoking this function.
// E.g. Slicer::reset(_waypoints, _currentWaypoints);
using namespace WaypointManager::Utils;
if (_waypoints.count() < 1 || !_settings->valid()) {
return false;
}
if (_dirty) {
_missionItems.clearAndDeleteContents();
_waypointsVariant.clear();
// No initialization of _missionItems, don't need MissionSettingsItem here.
for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
auto &c = _waypoints.at(i);
_insertMissionItem(c, _missionItems.count(), _missionItems,
false /*update list*/);
_waypointsVariant.push_back(QVariant::fromValue(c));
}
updateHirarchy(_missionItems);
updateSequenceNumbers(
_missionItems, 1); // Start with 1, since MissionSettingsItem missing.
_dirty = false;
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView());
// Calculate path from home to first waypoint.
QVector<QGeoCoordinate> arrivalPath;
if (!_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(),
arrivalPath)) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
"from home position to first waypoint.");
qWarning() << "from: " << _settings->homePosition();
qWarning() << "to: " << _currentWaypoints.first();
return false;
}
if (!arrivalPath.empty())
arrivalPath.removeFirst();
if (!arrivalPath.empty())
arrivalPath.removeLast();
// calculate path from last waypoint to home
QVector<QGeoCoordinate> returnPath;
if (!_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(),
returnPath)) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
"from home position to last waypoint.");
qWarning() << "from: " << _currentWaypoints.last();
qWarning() << "to: " << _settings->homePosition();
return false;
}
if (!returnPath.empty())
returnPath.removeFirst();
if (!returnPath.empty())
returnPath.removeLast();
// Create mission items.
// Set home position of MissionSettingsItem.
MissionSettingsItem *settingsItem =
_currentMissionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
settingsItem->setCoordinate(_settings->homePosition());
// First mission item is takeoff command.
_insertMissionItem(_settings->homePosition(), 1 /*insertion index*/,
false /*do update*/);
SimpleMissionItem *takeoffItem =
_currentMissionItems.value<SimpleMissionItem *>(1);
if (takeoffItem == nullptr) {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
Q_ASSERT(takeoffItem != nullptr);
return false;
}
makeTakeOffCmd(takeoffItem, _settings->masterController());
// Create change speed item.
_insertMissionItem(_settings->homePosition(), 2 /*insertion index*/,
false /*do update*/);
SimpleMissionItem *speedItem =
_currentMissionItems.value<SimpleMissionItem *>(2);
if (speedItem == nullptr) {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
Q_ASSERT(speedItem != nullptr);
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// insert arrival path
for (auto coordinate : arrivalPath) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item (after arrival path).
int index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.first(), index /*insertion index*/,
false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
if (speedItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeSpeedCmd(speedItem, _settings->flightSpeed());
// Insert slice.
for (auto coordinate : _currentWaypoints) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item, after slice.
index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.last(), index /*insertion index*/,
false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
if (speedItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Set land command for last mission item.
index = _currentMissionItems.count();
_insertMissionItem(_settings->homePosition(), index /*insertion index*/,
false /*do update*/);
SimpleMissionItem *landItem =
_currentMissionItems.value<SimpleMissionItem *>(index);
if (landItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item =
_currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
item->altitude()->setRawValue(_settings->altitude());
}
// Update list _currentMissionItems.
updateHirarchy(_currentMissionItems);
updateSequenceNumbers(_currentMissionItems);
// Prepend arrival path to slice.
for (long i = arrivalPath.size() - 1; i >= 0; --i)
_currentWaypoints.push_front(arrivalPath[i]);
_currentWaypoints.push_front(_settings->homePosition());
// Append return path to slice.
for (auto c : returnPath)
_currentWaypoints.push_back(c);
_currentWaypoints.push_back(_settings->homePosition());
// Create variant list.
_currentWaypointsVariant.clear();
for (auto c : _currentWaypoints)
_currentWaypointsVariant.push_back(QVariant::fromValue(c));
return true;
}
#pragma once
#include <QGeoCoordinate>
#include <QVector>
#include "GenericWaypointManager.h"
#include "QmlObjectListModel.h"
#include "Settings.h"
#include "AreaInterface.h"
namespace WaypointManager {
typedef GenericWaypointManager<QGeoCoordinate,
QVector,
QmlObjectListModel,
Settings>
ManagerBase;
//!
//! \brief The DefaultManager class is used to manage SimpleMissionItems.
//!
//! @note The \class QmlObjectList returned by \fn missionItems doesn't contain a MissionSettingsItem.
//! This list is supposed to be used for display purposes only.
class DefaultManager : public ManagerBase
{
public:
DefaultManager() = delete;
DefaultManager(Settings &settings,
AreaInterface &interface);
void clear() override;
virtual bool update() override;
virtual bool next() override;
virtual bool previous() override;
virtual bool reset() override;
protected:
bool _insertMissionItem(const QGeoCoordinate &c,
size_t index,
QmlObjectListModel &list,
bool doUpdate);
bool _insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate);
bool _calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
AreaInterface *_areaInterface;
private:
bool _worker();
};
} // namespace WaypointManager
#include "EmptyManager.h"
namespace WaypointManager {
WaypointManager::EmptyManager::EmptyManager(Settings &settings, AreaInterface &)
: ManagerBase(settings) {}
void WaypointManager::EmptyManager::clear() {}
bool WaypointManager::EmptyManager::update() { return true; }
bool WaypointManager::EmptyManager::next() { return true; }
bool WaypointManager::EmptyManager::previous() { return true; }
bool WaypointManager::EmptyManager::reset() { return true; }
} // namespace WaypointManager
#pragma once
#include <QGeoCoordinate>
#include <QVector>
#include "AreaInterface.h"
#include "GenericWaypointManager.h"
#include "QmlObjectListModel.h"
#include "Settings.h"
namespace WaypointManager {
typedef GenericWaypointManager<QGeoCoordinate, QVector, QmlObjectListModel,
Settings>
ManagerBase;
//!
//! \brief The EmptyManager is a place holder and does noting.
class EmptyManager : public ManagerBase {
public:
EmptyManager() = delete;
EmptyManager(Settings &settings, AreaInterface &);
void clear() override;
virtual bool update() override;
virtual bool next() override;
virtual bool previous() override;
virtual bool reset() override;
};
} // namespace WaypointManager
#pragma once
#include <iostream>
#include "Slicer.h"
namespace WaypointManager {
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
class GenericWaypointManager : public Slicer
{
public:
typedef ContainerType<WaypointType> WaypointList;
GenericWaypointManager() = delete;
GenericWaypointManager(SettingsType &settings);
// Waypoint editing.
virtual void setWaypoints(const WaypointList &waypoints);
virtual void push_back (const WaypointType &wp);
virtual void push_front (const WaypointType &wp);
virtual void clear ();
virtual void insert (std::size_t i, const WaypointType &wp);
virtual std::size_t size () const;
virtual void remove (std::size_t i);
const WaypointList &waypoints() const;
const WaypointList &currentWaypoints() const;
const MissionItemList &missionItems() const;
const MissionItemList &currentMissionItems() const;
const QVariantList &waypointsVariant() const;
const QVariantList &currentWaypointsVariant() const;
virtual bool update() = 0;
virtual bool next() = 0;
virtual bool previous() = 0;
virtual bool reset() = 0;
protected:
WaypointList _waypoints;
WaypointList _currentWaypoints;
MissionItemList _currentMissionItems;
MissionItemList _missionItems;
SettingsType *_settings;
bool _dirty;
QVariantList _waypointsVariant;
QVariantList _currentWaypointsVariant;
};