Commit 72acd11d authored by Valentin Platzgummer's avatar Valentin Platzgummer

Working on default waypointmanager, not done yet.

parent 51adc15a
......@@ -438,12 +438,14 @@ HEADERS += \
src/Wima/Snake/SnakeTiles.h \
src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/Snake/SnakeWorker.h \
src/Wima/WaypointManager/AreaInterface.h \
src/Wima/WaypointManager/DefaultManager.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/Wima/WaypointManager/Utils.h \
src/api/QGCCorePlugin.h \
src/api/QGCOptions.h \
src/api/QGCSettings.h \
......@@ -497,10 +499,12 @@ SOURCES += \
src/Wima/Geometry/PolygonArray.cc \
src/Wima/Snake/QNemoProgress.cc \
src/Wima/Snake/SnakeWorker.cc \
src/Wima/WaypointManager/AreaInterface.cpp \
src/Wima/WaypointManager/DefaultManager.cpp \
src/Wima/WaypointManager/GenericSlicer.cpp \
src/Wima/WaypointManager/GenericWaypointManager.cpp \
src/Wima/WaypointManager/Settings.cpp \
src/Wima/WaypointManager/utils.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 \
......
......@@ -2,11 +2,11 @@
#include "TransectStyleComplexItem.h"
#include "PolygonCalculus.h"
#include "PlanimetryCalculus.h"
#include "GeoUtilities.h"
#include "Geometry/PolygonCalculus.h"
#include "Geometry/PlanimetryCalculus.h"
#include "Geometry/GeoUtilities.h"
#include "QVector"
#include "Circle.h"
#include "Geometry/Circle.h"
#include "SettingsFact.h"
class CircularSurveyComplexItem :public TransectStyleComplexItem
......
#include "PolygonCalculus.h"
#include "PlanimetryCalculus.h"
#include "OptimisationTools.h"
#include "Wima/OptimisationTools.h"
#include <QVector3D>
......
......@@ -4,7 +4,6 @@
#include "QGCMapPolyline.h"
#include "Vehicle.h"
#include "qobject.h"
#include "WimaVehicle.h"
#include <QLineF>
#include <QPointF>
#include "QGCGeo.h"
......
......@@ -2,6 +2,7 @@
WimaAreaData::WimaAreaData(QObject *parent)
: QObject(parent)
, _listValid(false)
{
_maxAltitude = 0;
}
......@@ -31,14 +32,15 @@ QGeoCoordinate WimaAreaData::center() const
return _center;
}
QList<QGeoCoordinate> WimaAreaData::coordinateList() const
const QList<QGeoCoordinate> &WimaAreaData::coordinateList() const
{
QList<QGeoCoordinate> coordinateList;
for ( auto coorinate : _path)
coordinateList.append(coorinate.value<QGeoCoordinate>());
static QList<QGeoCoordinate> list;
if ( !_listValid ) {
for ( auto coorinate : _path)
list.append(coorinate.value<QGeoCoordinate>());
}
return coordinateList;
return list;
}
bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const
......@@ -54,6 +56,16 @@ bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const
return false;
}
void WimaAreaData::append(const QGeoCoordinate &c) {
_listValid = false;
_path.push_back(QVariant::fromValue(c));
}
void WimaAreaData::clear() {
_listValid = false;
_path.clear();
}
/*!
* \fn void WimaAreaData::setMaxAltitude(double maxAltitude)
......@@ -72,6 +84,7 @@ void WimaAreaData::setMaxAltitude(double maxAltitude)
void WimaAreaData::setPath(const QVariantList &coordinateList)
{
_listValid = false;
_path.clear();
_path.append(coordinateList);
}
......@@ -113,6 +126,7 @@ void WimaAreaData::assign(const WimaArea &other)
*/
void WimaAreaData::setPath(const QList<QGeoCoordinate> &coordinateList)
{
_listValid = false;
_path.clear();
// copy all coordinates to _path
......
......@@ -21,12 +21,12 @@ public:
double maxAltitude() const;
QVariantList path() const;
QGeoCoordinate center() const;
QList<QGeoCoordinate> coordinateList() const;
const QList<QGeoCoordinate> &coordinateList() const;
bool containsCoordinate(const QGeoCoordinate &coordinate) const;
virtual QString type() const = 0;
void append(const QGeoCoordinate &c) {_path.push_back(QVariant::fromValue(c));}
void clear() {_path.clear();}
void append(const QGeoCoordinate &c);
void clear();
signals:
void maxAltitudeChanged (double maxAltitude);
......@@ -51,5 +51,6 @@ private:
double _maxAltitude;
QVariantList _path;
QGeoCoordinate _center;
bool _listValid;
};
......@@ -51,7 +51,6 @@ signals:
void bottomLayerAltitudeChanged (void);
void numberOfLayersChanged (void);
void layerDistanceChanged (void);
void vehicleCorridorChanged (WimaVCorridor* corridor);
public slots:
void setBottomLayerAltitude (double altitude);
......
......@@ -38,7 +38,6 @@ public:
signals:
void takeOffPositionChanged (void);
void landPositionChanged (void);
void vehicleCorridorChanged (WimaVCorridor& corridor);
public slots:
void setTakeOffPosition (const QGeoCoordinate& coordinate);
......
#pragma once
#include "snaketile.h"
#include "WimaPolygonArray.h"
#include "Wima/Geometry/WimaPolygonArray.h"
typedef WimaPolygonArray<SnakeTile, QVector> SnakeTiles;
#pragma once
#include "ros_bridge/include/MessageGroups.h"
#include "Polygon2D.h"
#include "WimaPolygonArray.h"
#include "Wima/Geometry/Polygon2D.h"
#include "Wima/Geometry/WimaPolygonArray.h"
namespace MsgGroups = ROSBridge::MessageGroups;
typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> SnakeTilesLocal;
#pragma once
#include "WimaAreaData.h"
#include "Wima/Geometry/WimaAreaData.h"
class SnakeTile : public WimaAreaData
{
......
#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 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"
bool WaypointManager::DefaultManager::update()
{
// extract waypoints
_slice.clear();
_slicer->update(_waypoints, _slice);
return _worker();
}
bool WaypointManager::DefaultManager::next()
{
// extract waypoints
_slice.clear();
_slicer->next(_waypoints, _slice);
return _worker();
}
bool WaypointManager::DefaultManager::previous()
{
// extract waypoints
_slice.clear();
_slicer->previous(_waypoints, _slice);
return _worker();
}
bool WaypointManager::DefaultManager::reset()
{
// extract waypoints
_slice.clear();
_slicer->reset(_waypoints, _slice);
return _worker();
}
bool WaypointManager::DefaultManager::_insertMissionItem(size_t index, const QGeoCoordinate &c, bool doUpdate)
{
using namespace WaypointManager::Utils;
if ( !insertMissionItem(c,
index /*insertion index*/,
_missionItems,
_settings->vehicle(),
_settings->isFlyView(),
&_missionItems /*parent*/,
doUpdate /*do update*/) )
{
assert(false);
qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed.");
return false;
}
return true;
}
bool WaypointManager::DefaultManager::_calcShortestPath(
const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QVector<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(
toQPolygonF(toCartesian2D(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start)),
/*start point*/ QPointF(0,0),
/*destination*/ toCartesian2D(destination, start),
/*shortest path*/ path2D);
path.append(toGeo(path2D, /*origin*/ start));
return retVal;
}
bool WaypointManager::DefaultManager::_worker()
{
using namespace WaypointManager::Utils;
if (_waypoints.count() < 1) {
return false;
}
_missionItems.clearAndDeleteContents();
initialize(_missionItems, _settings->vehicle(), _settings->isFlyView());
// calculate path from home to first waypoint
QVector<QGeoCoordinate> arrivalPath;
if (!_settings->homePosition().isValid()){
assert(false);
qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
return false;
}
if ( !_calcShortestPath(_settings->homePosition(), _slice.first(), arrivalPath) ) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
return false;
}
arrivalPath.removeFirst();
// calculate path from last waypoint to home
QVector<QGeoCoordinate> returnPath;
if ( !_calcShortestPath(_slice.last(), _settings->homePosition(), returnPath) ) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint.");
return false;
}
returnPath.removeFirst();
returnPath.removeLast();
// Create mission items.
// Set home position of MissionSettingsItem.
MissionSettingsItem* settingsItem = _missionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
settingsItem->setCoordinate(_settings->homePosition());
// Set takeoff position for first mission item (bug).
_insertMissionItem(1 /*insertion index*/, _settings->homePosition(), false /*do update*/);
SimpleMissionItem *takeoffItem = _missionItems.value<SimpleMissionItem*>(1);
if (takeoffItem == nullptr) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
takeoffItem->setCoordinate(_settings->homePosition());
// Create change speed item.
_insertMissionItem(2 /*insertion index*/, _settings->homePosition(), false /*do update*/);
SimpleMissionItem *speedItem = _missionItems.value<SimpleMissionItem*>(2);
if (speedItem == nullptr) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
// Set coordinate must be after setCommand (setCommand sets coordinate to zero).
speedItem->setCoordinate(_settings->homePosition());
speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());
// insert arrival path
for (auto coordinate : arrivalPath) {
_insertMissionItem(_missionItems.count() /*insertion index*/,
coordinate,
false /*do update*/);
}
// Create change speed item (after arrival path).
int index = _missionItems.count();
_insertMissionItem(index /*insertion index*/, _slice.first(), false /*do update*/);
speedItem = _missionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_slice.first());
speedItem->missionItem().setParam2(_settings->flightSpeed());
// Insert slice.
for (auto coordinate : _slice) {
_insertMissionItem(_missionItems.count() /*insertion index*/,
coordinate,
false /*do update*/);
}
// Create change speed item, after slice.
index = _missionItems.count();
_insertMissionItem(index /*insertion index*/, _slice.last(), false /*do update*/);
speedItem = _missionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_slice.last());
speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
_insertMissionItem(_missionItems.count() /*insertion index*/,
coordinate,
false /*do update*/);
}
// Set land command for last mission item.
index = _missionItems.count();
_insertMissionItem(index /*insertion index*/, _settings->homePosition(), false /*do update*/);
SimpleMissionItem *landItem = _missionItems.value<SimpleMissionItem*>(index);
if (landItem == nullptr) {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
MAV_CMD landCmd = _settings->vehicle()->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
landItem->setCommand(landCmd);
} else {
assert(false);
qWarning("WaypointManager::DefaultManager::next(): Land command not supported!");
return false;
}
// Prepend arrival path to slice.
for ( long i = arrivalPath.size()-1; i >=0; --i )
_slice.push_front(arrivalPath[i]);
// Append return path to slice.
for ( auto c : returnPath )
_slice.push_back(c);
// Set altitude.
for (int i = 0; i < _missionItems.count(); ++i) {
SimpleMissionItem *item = _missionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
assert(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
item->altitude()->setRawValue(_settings->altitude());
}
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;
class DefaultManager : public ManagerBase
{
public:
DefaultManager() = delete;
DefaultManager(Slicer *slicer,
Settings *settings,
AreaInterface *interface);
virtual bool update() override;
virtual bool next() override;
virtual bool previous() override;
virtual bool reset() override;
protected:
bool _insertMissionItem(size_t index, const QGeoCoordinate &c, bool doUpdate);
bool _calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
AreaInterface *_areaInterface;
private:
bool _worker();
};
} // namespace WaypointManager
......@@ -3,7 +3,7 @@
#include <assert.h>
#include <iostream>
#include "utils.h"
#include "Utils.h"
//! @brief Base class for all waypoint managers.
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
......@@ -15,10 +15,10 @@ public:
// Slicing.
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);
void update (const ContainerType &source, ContainerType &update);
void next (const ContainerType &source, ContainerType &update);
void previous (const ContainerType &source, ContainerType &update);
void reset (const ContainerType &source, ContainerType &update);
// Slicing parameters.
......@@ -134,7 +134,7 @@ void GenericSlicer<ElementType, Container>::_updateIdx(std::size_t size)
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericSlicer<ElementType, Container>::slice(const ContainerType &source,
void GenericSlicer<ElementType, Container>::update(const ContainerType &source,
Container<ElementType> &slice){
if ( !_idxValid)
......
......@@ -3,59 +3,186 @@
#include <iostream>
#include "GenericSlicer.h"
#include "Settings.h"
namespace WaypointManager {
template<class WaypointType,
template<class, class...> class ContainerType>
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
class GenericWaypointManager
{
typedef GenericSlicer<WaypointType, ContainerType> Slicer;
public:
typedef GenericSlicer<WaypointType, ContainerType> Slicer;
typedef ContainerType<WaypointType> WaypointList;
GenericWaypointManager() = delete;
GenericWaypointManager(Slicer *slicer, Settings *settings);
GenericWaypointManager(Slicer *slicer, SettingsType *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);
void insert (std::size_t i, const WaypointType &wp);
std::size_t size () const;
WaypointType &at (std::size_t i);
const WaypointList &waypoints() const;
const WaypointList &slice() const;
const MissionItemList &missionItems() const;
virtual bool update() = 0;
virtual bool next() = 0;
virtual bool previous() = 0;
virtual bool reset() = 0;
protected:
WaypointList _waypoints;
WaypointList _slice;
MissionItemList _missionItems;
Slicer *_slicer;
SettingsType *_settings;
};
const ContainerType &getWaypoints () ;
const ContainerType &getSlcieaypoints () ;
const ContainerType &missionItems () ;
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::GenericWaypointManager(
Slicer *slicer,
SettingsType *Settings
)
: _slicer(slicer)
, _settings(_settings)
{}
void slice();
void next();
void previous();
void reset();
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::push_back(const WaypointType &wp)
{
_waypoints.push_back(wp);
}
private:
WaypointList