Skip to content
GitLab
Commit 7427e2f7 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files

Waypoint managers removed, code compiles

parent 3dc41359
Hide whitespace changes
Inline Side-by-side
qgroundcontrol.pro
View file @ 7427e2f7
......@@ -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 \
......
src/Wima/Geometry/WimaCorridor.cc
View file @ 7427e2f7
......@@ -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
......
src/Wima/Geometry/WimaCorridor.h
View file @ 7427e2f7
......@@ -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:
......
src/Wima/Geometry/WimaJoinedArea.cc
View file @ 7427e2f7
......@@ -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.
......
src/Wima/Geometry/WimaJoinedArea.h
View file @ 7427e2f7
......@@ -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:
......
src/Wima/Geometry/WimaServiceArea.cc
View file @ 7427e2f7
......@@ -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] {
......
src/Wima/Geometry/WimaServiceArea.h
View file @ 7427e2f7
......@@ -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;
......
src/Wima/WaypointManager/AreaInterface.cpp deleted 100644 → 0
View file @ 3dc41359
#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;
}
src/Wima/WaypointManager/AreaInterface.h deleted 100644 → 0
View file @ 3dc41359
#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 deleted 100644 → 0
View file @ 3dc41359
#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;
}
src/Wima/WaypointManager/DefaultManager.h deleted 100644 → 0
View file @ 3dc41359
#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
src/Wima/WaypointManager/EmptyManager.cpp deleted 100644 → 0
View file @ 3dc41359
#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
src/Wima/WaypointManager/EmptyManager.h deleted 100644 → 0
View file @ 3dc41359
#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
src/Wima/WaypointManager/GenericWaypointManager.h deleted 100644 → 0
View file @ 3dc41359
#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;
};
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::GenericWaypointManager(SettingsType &settings)
: Slicer()
, _settings(&settings)
, _dirty(true)
{}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::push_back(const WaypointType &wp)
{
_dirty = true;
_waypoints.push_back(wp);
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::push_front(
const WaypointType &wp)
{
_dirty = true;
_waypoints.push_front(wp);
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::clear()
{
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clear();
_currentMissionItems.clear();
_waypointsVariant.clear();
_currentWaypointsVariant.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)
{
_dirty = true;
_waypoints.insert(i, wp);
}
template<class WaypointType,
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>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::remove(std::size_t i)
{
return _waypoints.remove(i);
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const QVariantList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::waypointsVariant() const
{
return _waypointsVariant;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const QVariantList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::currentWaypointsVariant() const
{
return _currentWaypointsVariant;
}
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>::currentWaypoints() const
{
return _currentWaypoints;
}
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>
const MissionItemList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::currentMissionItems() const
{
return _currentMissionItems;
}
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;
}
} // namespace WaypointManager
src/Wima/WaypointManager/RTLManager.cpp deleted 100644 → 0
View file @ 3dc41359
#include "RTLManager.h"
#include "Wima/Geometry/GeoUtilities.h"
#include "Wima/Geometry/PolygonCalculus.h"
#include "MissionSettingsItem.h"
#include "SimpleMissionItem.h"
WaypointManager::RTLManager::RTLManager(Settings &settings,
AreaInterface &interface)
: ManagerBase(settings), _areaInterface(&interface) {}
void WaypointManager::RTLManager::setWaypoints(
const QVector<QGeoCoordinate> &waypoints) {
(void)waypoints;
return;
}
void WaypointManager::RTLManager::push_back(const QGeoCoordinate &wp) {
(void)wp;
return;
}
void WaypointManager::RTLManager::push_front(const QGeoCoordinate &wp) {
(void)wp;
return;
}
void WaypointManager::RTLManager::clear() {
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
}
void WaypointManager::RTLManager::insert(std::size_t i,
const QGeoCoordinate &wp) {
(void)i;
(void)wp;
return;
}
std::size_t WaypointManager::RTLManager::size() const { return 0; }
void WaypointManager::RTLManager::remove(std::size_t i) { (void)i; }
bool WaypointManager::RTLManager::update() {
this->clear();
return _worker();
}
bool WaypointManager::RTLManager::next() { return true; }
bool WaypointManager::RTLManager::previous() { return true; }
bool WaypointManager::RTLManager::reset() { return true; }
bool WaypointManager::RTLManager::checkPrecondition(QString &errorString) {
// Be aware of the error message order!
Vehicle *managerVehicle = _settings->masterController()->managerVehicle();
if (managerVehicle->isOfflineEditingVehicle()) {
errorString.append("No vehicle connected. Smart RTL not available.");
return false;
}
if (!managerVehicle->flying()) {
errorString.append("Vehicle is not flying. Smart RTL not available.");
return false;
}
if (!_areaInterface->joinedArea()->containsCoordinate(
managerVehicle->coordinate())) {
errorString.append(
"Vehicle not inside save area. Smart RTL not available.");
return false;
}
return true;
}
bool WaypointManager::RTLManager::_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::RTLManager::next(): insertMissionItem failed.");
Q_ASSERT(false);
return false;
}
return true;
}
bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate) {
return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}
bool WaypointManager::RTLManager::_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::RTLManager::_worker() {
// Precondition: settings must be valid.
using namespace WaypointManager::Utils;
if (!_settings->valid()) {
return false;
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView());
// Calculate path from vehicle to home position.
QVector<QGeoCoordinate> returnPath;
auto vehicleCoordinate =
_settings->masterController()->managerVehicle()->coordinate();
if (!_calcShortestPath(vehicleCoordinate, _settings->homePosition(),
returnPath)) {
qWarning("WaypointManager::RTLManager::next(): Not able to calc path from "
"vehicle to home position.");
return false;
}
// Create mission items.
// Set home position of MissionSettingsItem.
MissionSettingsItem *settingsItem =
_currentMissionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::RTLManager::next(): nullptr.");
return false;
}
settingsItem->setCoordinate(vehicleCoordinate);
// Create change speed item.
int index = _currentMissionItems.count();
_insertMissionItem(vehicleCoordinate, index /*insertion index*/,
false /*do update*/);
SimpleMissionItem *speedItem =
_currentMissionItems.value<SimpleMissionItem *>(index);
if (speedItem == nullptr) {
qWarning("WaypointManager::RTLManager::next(): nullptr.");
Q_ASSERT(speedItem != nullptr);
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
index = _currentMissionItems.count();
_insertMissionItem(coordinate, index /*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::RTLManager::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);
// Append return path to _currentWaypoints.
for (auto c : returnPath)
_currentWaypoints.push_back(c);
// Create variant list.
_currentWaypointsVariant.clear();
for (auto c : _currentWaypoints)
_currentWaypointsVariant.push_back(QVariant::fromValue(c));
return true;
}
src/Wima/WaypointManager/RTLManager.h deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include "GenericWaypointManager.h"
#include "Settings.h"
#include "AreaInterface.h"
namespace WaypointManager {
typedef GenericWaypointManager<QGeoCoordinate,
QVector,
QmlObjectListModel,
Settings>
ManagerBase;
//!
//! \brief The RTLManager class is used to manage the return to launch of the vehicle.
//!
//! @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 RTLManager : public ManagerBase
{
public:
RTLManager() = delete;
RTLManager(Settings &settings,
AreaInterface &interface);
//!
//! \brief Does nothing.
//!
virtual void setWaypoints(const QVector<QGeoCoordinate> &waypoints) override;
//!
//! \brief Does nothing.
//!
virtual void push_back (const QGeoCoordinate &wp) override;
//!
//! \brief Does nothing.
//!
virtual void push_front (const QGeoCoordinate &wp) override;
//!
//! \brief Clears the waypoint manager.
virtual void clear () override;
//!
//! \brief Does nothing.
//!
virtual void insert (std::size_t i, const QGeoCoordinate &wp) override;
//!
//! \brief Returns zero.
//! \return Returns zero.
virtual std::size_t size () const override;
//!
//! \brief Returns an empty coordinate.
virtual void remove (std::size_t i) override;
//!
//! \brief Updates the waypoint manager.
//!
//! After updateing a call to currentMissionItems or currentWaypoints returns
//! a path from the vehicles current position to the home position.
//!
//! \return Returns true on success, false either.
//!
virtual bool update() override;
//!
//! \brief Does nothing.
//! \return Returns true.
//!
virtual bool next() override;
//!
//! \brief Does nothing.
//! \return Returns true.
//!
virtual bool previous() override;
//!
//! \brief Does nothing.
//! \return Returns true.
//!
virtual bool reset() override;
bool checkPrecondition(QString &errorString);
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
src/Wima/WaypointManager/Settings.cpp deleted 100644 → 0
View file @ 3dc41359
#include "Settings.h"
WaypointManager::Settings::Settings()
: _missionController(nullptr)
, _isFlyView(true)
, _arrivalReturnSpeed(5)
, _flightSpeed(2)
, _altitude(5)
{
}
bool WaypointManager::Settings::valid() const
{
return _missionController != nullptr
&& _homePosition.isValid();
}
void WaypointManager::Settings::setHomePosition(const QGeoCoordinate &c)
{
_homePosition = c;
}
void WaypointManager::Settings::setMissionController(MissionController *controller)
{
_missionController = controller;
}
void WaypointManager::Settings::setMasterController(PlanMasterController *controller)
{
_masterController = controller;
}
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;
}
QGeoCoordinate &WaypointManager::Settings::homePosition()
{
return _homePosition;
}
MissionController *WaypointManager::Settings::missionController() const
{
return _missionController;
}
PlanMasterController *WaypointManager::Settings::masterController() const
{
return _masterController;
}
Vehicle *WaypointManager::Settings::vehicle() const
{
return _masterController->managerVehicle();
}
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 deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include <QGeoCoordinate>
#include "MissionController.h"
#include "PlanMasterController.h"
#include "Vehicle.h"
namespace WaypointManager {
class Settings
{
public:
Settings();
bool valid() const;
void setHomePosition(const QGeoCoordinate &c);
void setMissionController(MissionController *controller);
void setMasterController(PlanMasterController *controller);
void setIsFlyView(bool isFlyView);
void setArrivalReturnSpeed(double speed);
void setFlightSpeed(double speed);
void setAltitude(double altitude);
const QGeoCoordinate &homePosition() const;
QGeoCoordinate &homePosition();
MissionController *missionController() const;
PlanMasterController *masterController() const;
Vehicle *vehicle() const;
bool isFlyView() const;
double arrivalReturnSpeed() const;
double flightSpeed() const;
double altitude() const;
private:
QGeoCoordinate _homePosition;
MissionController *_missionController;
PlanMasterController *_masterController;
bool _isFlyView;
double _arrivalReturnSpeed;
double _flightSpeed;
double _altitude;
};
} // namespace WaypointManager
src/Wima/WaypointManager/Slicer.cpp deleted 100644 → 0
View file @ 3dc41359
#include "Slicer.h"
Slicer::Slicer()
: _idxStart(0)
, _idxEnd(0)
, _idxNext(0)
, _idxPrevious(0)
, _overlap(0)
, _N(0)
{}
void Slicer::setOverlap(uint32_t overlap)
{
_overlap = overlap;
}
void Slicer::setN(uint32_t N)
{
_N = N > 0 ? N : 1;
}
void Slicer::setStartIndex(int idxStart)
{
_idxStart = idxStart;
}
uint32_t Slicer::overlap()
{
return _overlap;
}
uint32_t Slicer::N()
{
return _N;
}
int Slicer::startIndex()
{
return _idxStart;
}
void Slicer::_updateIdx(long size)
{
_overlap = _overlap < _N ? _overlap : _N-1;
_idxStart = _idxStart < size ? _idxStart : size-1;
_idxStart = _idxStart < 0 ? 0 : _idxStart;
_idxEnd = _idxStart + _N - 1;
_idxEnd = _idxEnd < size ? _idxEnd : size-1;
_idxNext = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap;
_idxNext = _idxNext < 0 ? 0 : _idxNext;
_idxNext = _idxNext < size ? _idxNext : size-1;
_idxPrevious = _idxStart + _overlap - _N;
_idxPrevious = _idxPrevious < 0 ? 0 : _idxPrevious;
_idxPrevious = _idxPrevious < size ? _idxPrevious : size-1;
// qDebug() << "size: " << size;
// qDebug() << "_N: " << _N;
// qDebug() << "_overlap: " << _overlap;
// qDebug() << "_idxStart: " << _idxStart;
// qDebug() << "_idxEnd: " << _idxEnd;
// qDebug() << "_idxNext: " << _idxNext;
// qDebug() << "_idxPrevious: " << _idxPrevious << "\n";
}
Supports Markdown
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