Commit 6a082ad8 authored by Valentin Platzgummer's avatar Valentin Platzgummer

Wima directory structure improved

parent d02c3c46
...@@ -429,50 +429,49 @@ HEADERS += \ ...@@ -429,50 +429,49 @@ HEADERS += \
src/Snake/snake.h \ src/Snake/snake.h \
src/Snake/snake_geometry.h \ src/Snake/snake_geometry.h \
src/Snake/snake_global.h \ src/Snake/snake_global.h \
src/Wima/GeoPoint3D.h \ src/Wima/Geometry/GeoPoint3D.h \
src/Wima/Polygon2D.h \ src/Wima/Geometry/Polygon2D.h \
src/Wima/PolygonArray.h \ src/Wima/Geometry/PolygonArray.h \
src/Wima/QNemoProgress.h \ src/Wima/Snake/QNemoProgress.h \
src/Wima/QtROSJsonFactory.h \ src/Wima/Snake/QtROSJsonFactory.h \
src/Wima/QtROSTypeFactory.h \ src/Wima/Snake/QtROSTypeFactory.h \
src/Wima/SnakeTiles.h \ src/Wima/Snake/SnakeTiles.h \
src/Wima/SnakeTilesLocal.h \ src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/SnakeWorker.h \ src/Wima/Snake/SnakeWorker.h \
src/Wima/WaypointManager/GenericPathSlicer.h \ src/Wima/WaypointManager/GenericPathSlicer.h \
src/Wima/WaypointManager/GenericWaypointManager.h \ src/Wima/WaypointManager/GenericWaypointManager.h \
src/Wima/WimaPolygonArray.h \ src/Wima/Geometry/WimaPolygonArray.h \
src/Wima/snaketile.h \ src/Wima/Snake/snaketile.h \
src/api/QGCCorePlugin.h \ src/api/QGCCorePlugin.h \
src/api/QGCOptions.h \ src/api/QGCOptions.h \
src/api/QGCSettings.h \ src/api/QGCSettings.h \
src/api/QmlComponentInfo.h \ src/api/QmlComponentInfo.h \
src/comm/MavlinkMessagesTimer.h \ src/comm/MavlinkMessagesTimer.h \
src/GPS/Drivers/src/base_station.h \ src/GPS/Drivers/src/base_station.h \
src/Wima/WimaArea.h \ src/Wima/Geometry/WimaArea.h \
src/Wima/WimaServiceArea.h \ src/Wima/Geometry/WimaServiceArea.h \
src/Wima/WimaTrackerPolyline.h \ src/Wima/Geometry/WimaTrackerPolyline.h \
src/Wima/WimaController.h \ src/Wima/WimaController.h \
src/Wima/WimaVehicle.h \
src/Wima/WimaDataContainer.h \ src/Wima/WimaDataContainer.h \
src/Wima/WimaPlaner.h \ src/Wima/WimaPlaner.h \
src/Wima/WimaMeasurementArea.h \ src/Wima/Geometry/WimaMeasurementArea.h \
src/Wima/WimaCorridor.h \ src/Wima/Geometry/WimaCorridor.h \
src/Wima/WimaAreaData.h \ src/Wima/Geometry/WimaAreaData.h \
src/Wima/WimaServiceAreaData.h \ src/Wima/Geometry/WimaServiceAreaData.h \
src/Wima/WimaCorridorData.h \ src/Wima/Geometry/WimaCorridorData.h \
src/Wima/WimaMeasurementAreaData.h \ src/Wima/Geometry/WimaMeasurementAreaData.h \
src/Wima/WimaPlanData.h \ src/Wima/WimaPlanData.h \
src/Wima/WimaJoinedArea.h \ src/Wima/Geometry/WimaJoinedArea.h \
src/Wima/WimaJoinedAreaData.h \ src/Wima/Geometry/WimaJoinedAreaData.h \
src/Wima/SphereCalculus.h \ src/Wima/Geometry/SphereCalculus.h \
src/Wima/CircularSurveyComplexItem.h \ src/Wima/CircularSurveyComplexItem.h \
src/Wima/PlanimetryCalculus.h \ src/Wima/Geometry/PlanimetryCalculus.h \
src/Wima/Circle.h \ src/Wima/Geometry/Circle.h \
src/Wima/PolygonCalculus.h \ src/Wima/Geometry/PolygonCalculus.h \
src/Wima/OptimisationTools.h \ src/Wima/OptimisationTools.h \
src/Wima/GeoUtilities.h \ src/Wima/Geometry/GeoUtilities.h \
src/Wima/TestPolygonCalculus.h \ src/Wima/Geometry/TestPolygonCalculus.h \
src/Wima/testplanimetrycalculus.h \ src/Wima/Geometry/testplanimetrycalculus.h \
src/Settings/WimaSettings.h \ src/Settings/WimaSettings.h \
src/QmlControls/QmlObjectVectorModel.h \ src/QmlControls/QmlObjectVectorModel.h \
src/comm/ros_bridge/include/CasePacker.h \ src/comm/ros_bridge/include/CasePacker.h \
...@@ -492,10 +491,10 @@ SOURCES += \ ...@@ -492,10 +491,10 @@ SOURCES += \
src/Snake/clipper/clipper.cpp \ src/Snake/clipper/clipper.cpp \
src/Snake/snake.cpp \ src/Snake/snake.cpp \
src/Snake/snake_geometry.cpp \ src/Snake/snake_geometry.cpp \
src/Wima/GeoPoint3D.cpp \ src/Wima/Geometry/GeoPoint3D.cpp \
src/Wima/PolygonArray.cc \ src/Wima/Geometry/PolygonArray.cc \
src/Wima/QNemoProgress.cc \ src/Wima/Snake/QNemoProgress.cc \
src/Wima/SnakeWorker.cc \ src/Wima/Snake/SnakeWorker.cc \
src/Wima/WaypointManager/GenericPathSlicer.cpp \ src/Wima/WaypointManager/GenericPathSlicer.cpp \
src/Wima/WaypointManager/GenericWaypointManager.cpp \ src/Wima/WaypointManager/GenericWaypointManager.cpp \
src/comm/ros_bridge/include/ComPrivateInclude.cpp \ src/comm/ros_bridge/include/ComPrivateInclude.cpp \
...@@ -503,37 +502,36 @@ SOURCES += \ ...@@ -503,37 +502,36 @@ SOURCES += \
src/comm/ros_bridge/include/TopicPublisher.cpp \ src/comm/ros_bridge/include/TopicPublisher.cpp \
src/comm/ros_bridge/include/TopicSubscriber.cpp \ src/comm/ros_bridge/include/TopicSubscriber.cpp \
src/comm/ros_bridge/src/CasePacker.cpp \ src/comm/ros_bridge/src/CasePacker.cpp \
src/Wima/snaketile.cpp \ src/Wima/Snake/snaketile.cpp \
src/api/QGCCorePlugin.cc \ src/api/QGCCorePlugin.cc \
src/api/QGCOptions.cc \ src/api/QGCOptions.cc \
src/api/QGCSettings.cc \ src/api/QGCSettings.cc \
src/api/QmlComponentInfo.cc \ src/api/QmlComponentInfo.cc \
src/comm/MavlinkMessagesTimer.cc \ src/comm/MavlinkMessagesTimer.cc \
src/Wima/WimaArea.cc \ src/Wima/Geometry/WimaArea.cc \
src/Wima/WimaServiceArea.cc \ src/Wima/Geometry/WimaServiceArea.cc \
src/Wima/WimaTrackerPolyline.cc \ src/Wima/Geometry/WimaTrackerPolyline.cc \
src/Wima/WimaController.cc \ src/Wima/WimaController.cc \
src/Wima/WimaVehicle.cc \
src/Wima/WimaDataContainer.cc \ src/Wima/WimaDataContainer.cc \
src/Wima/WimaPlaner.cc \ src/Wima/WimaPlaner.cc \
src/Wima/WimaMeasurementArea.cc \ src/Wima/Geometry/WimaMeasurementArea.cc \
src/Wima/WimaCorridor.cc \ src/Wima/Geometry/WimaCorridor.cc \
src/Wima/WimaAreaData.cc \ src/Wima/Geometry/WimaAreaData.cc \
src/Wima/WimaServiceAreaData.cc \ src/Wima/Geometry/WimaServiceAreaData.cc \
src/Wima/WimaCorridorData.cpp \ src/Wima/Geometry/WimaCorridorData.cpp \
src/Wima/WimaPlanData.cc \ src/Wima/WimaPlanData.cc \
src/Wima/WimaMeasurementAreaData.cc \ src/Wima/Geometry/WimaMeasurementAreaData.cc \
src/Wima/WimaJoinedArea.cc \ src/Wima/Geometry/WimaJoinedArea.cc \
src/Wima/WimaJoinedAreaData.cc \ src/Wima/Geometry/WimaJoinedAreaData.cc \
src/Wima/SphereCalculus.cc \ src/Wima/Geometry/SphereCalculus.cc \
src/Wima/CircularSurveyComplexItem.cc \ src/Wima/CircularSurveyComplexItem.cc \
src/Wima/PlanimetryCalculus.cc \ src/Wima/Geometry/PlanimetryCalculus.cc \
src/Wima/Circle.cc \ src/Wima/Geometry/Circle.cc \
src/Wima/OptimisationTools.cc \ src/Wima/OptimisationTools.cc \
src/Wima/GeoUtilities.cc \ src/Wima/Geometry/GeoUtilities.cc \
src/Wima/PolygonCalculus.cc \ src/Wima/Geometry/PolygonCalculus.cc \
src/Wima/TestPolygonCalculus.cpp \ src/Wima/Geometry/TestPolygonCalculus.cpp \
src/Wima/testplanimetrycalculus.cpp \ src/Wima/Geometry/testplanimetrycalculus.cpp \
src/Settings/WimaSettings.cc \ src/Settings/WimaSettings.cc \
src/QmlControls/QmlObjectVectorModel.cc \ src/QmlControls/QmlObjectVectorModel.cc \
src/comm/ros_bridge/src/ROSBridge.cpp src/comm/ros_bridge/src/ROSBridge.cpp
......
...@@ -275,10 +275,10 @@ ...@@ -275,10 +275,10 @@
<file alias="Vehicle/VibrationFact.json">src/Vehicle/VibrationFact.json</file> <file alias="Vehicle/VibrationFact.json">src/Vehicle/VibrationFact.json</file>
<file alias="Vehicle/WindFact.json">src/Vehicle/WindFact.json</file> <file alias="Vehicle/WindFact.json">src/Vehicle/WindFact.json</file>
<file alias="Video.SettingsGroup.json">src/Settings/Video.SettingsGroup.json</file> <file alias="Video.SettingsGroup.json">src/Settings/Video.SettingsGroup.json</file>
<file alias="WimaMeasurementArea.SettingsGroup.json">src/Wima/WimaMeasurementArea.SettingsGroup.json</file> <file alias="WimaMeasurementArea.SettingsGroup.json">src/Wima/Geometry/json/WimaMeasurementArea.SettingsGroup.json</file>
<file alias="CircularSurvey.SettingsGroup.json">src/Wima/CircularSurvey.SettingsGroup.json</file> <file alias="CircularSurvey.SettingsGroup.json">src/Wima/json/CircularSurvey.SettingsGroup.json</file>
<file alias="WimaArea.SettingsGroup.json">src/Wima/WimaArea.SettingsGroup.json</file> <file alias="WimaArea.SettingsGroup.json">src/Wima/Geometry/json/WimaArea.SettingsGroup.json</file>
<file alias="WimaController.SettingsGroup.json">src/Wima/WimaController.SettingsGroup.json</file> <file alias="WimaController.SettingsGroup.json">src/Wima/json/WimaController.SettingsGroup.json</file>
<file alias="Wima.SettingsGroup.json">src/Settings/Wima.SettingsGroup.json</file> <file alias="Wima.SettingsGroup.json">src/Settings/Wima.SettingsGroup.json</file>
</qresource> </qresource>
<qresource prefix="/MockLink"> <qresource prefix="/MockLink">
......
...@@ -244,8 +244,8 @@ FlightMap { ...@@ -244,8 +244,8 @@ FlightMap {
// Add Snake tiles center points to the map // Add Snake tiles center points to the map
MapItemView { MapItemView {
property bool _lengthMatching: wimaController.snakeTileCenterPoints.length property bool _lengthMatching: wimaController.snakeTileCenterPoints.count
=== wimaController.snakeProgress.length > 0
property bool _enable: wimaController.enableWimaController.value property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value && wimaController.enableSnake.value
......
#include "Circle.h"
Circle::Circle()
{
}
#include "GeoPoint3D.h"
GeoPoint3D *GeoPoint3D::Clone() const
{
return new GeoPoint3D(*this);
}
GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
{
this->setLatitude(p.latitude());
this->setLongitude(p.longitude());
this->setAltitude(p.altitude());
return *this;
}
#include "GeoPoint3D.h"
GeoPoint3D::GeoPoint3D(QObject *parent)
: QObject(parent), ROSGeoPoint() {}
GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent)
: QObject(parent), ROSGeoPoint(latitude, longitude, altitude)
{}
GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D *GeoPoint3D::Clone() const
{
return new GeoPoint3D(*this);
}
GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
{
this->setLatitude(p.latitude());
this->setLongitude(p.longitude());
this->setAltitude(p.altitude());
return *this;
}
GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p)
{
this->setLatitude(p.latitude());
this->setLongitude(p.longitude());
this->setAltitude(p.altitude());
return *this;
}
...@@ -4,7 +4,10 @@ ...@@ -4,7 +4,10 @@
#include "ros_bridge/include/MessageBaseClass.h" #include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/GenericMessages.h" #include "ros_bridge/include/GenericMessages.h"
#include <QGeoCoordinate>
#include <QObject> #include <QObject>
typedef ROSBridge::MessageBaseClass ROSMsg; typedef ROSBridge::MessageBaseClass ROSMsg;
typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint; typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
namespace MsgGroups = ROSBridge::MessageGroups; namespace MsgGroups = ROSBridge::MessageGroups;
...@@ -14,24 +17,22 @@ class GeoPoint3D : public QObject, public ROSGeoPoint ...@@ -14,24 +17,22 @@ class GeoPoint3D : public QObject, public ROSGeoPoint
public: public:
typedef MsgGroups::GeoPointGroup Group; typedef MsgGroups::GeoPointGroup Group;
explicit GeoPoint3D(QObject *parent = nullptr) GeoPoint3D(QObject *parent = nullptr);
: QObject(parent), ROSGeoPoint() {} GeoPoint3D(double latitude,
explicit GeoPoint3D(double latitude, double longitude,
double longitude, double altitude,
double altitude, QObject *parent = nullptr);
QObject *parent = nullptr) GeoPoint3D(const GeoPoint3D& p,
: QObject(parent), ROSGeoPoint(latitude, longitude, altitude) QObject *parent = nullptr);
{} GeoPoint3D(const ROSGeoPoint& p,
explicit GeoPoint3D(const GeoPoint3D& p, QObject *parent = nullptr);
QObject *parent = nullptr) GeoPoint3D(const QGeoCoordinate& p,
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude()) QObject *parent = nullptr);
{}
explicit GeoPoint3D(const ROSGeoPoint& p,
QObject *parent = nullptr)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
virtual GeoPoint3D *Clone() const override; virtual GeoPoint3D *Clone() const override;
GeoPoint3D &operator=(const GeoPoint3D&p); GeoPoint3D &operator=(const GeoPoint3D&p);
GeoPoint3D &operator=(const QGeoCoordinate&p);
}; };
...@@ -111,6 +111,14 @@ WimaController::WimaController(QObject *parent) ...@@ -111,6 +111,14 @@ WimaController::WimaController(QObject *parent)
_startStopRosBridge(); _startStopRosBridge();
} }
QVariantList WimaController::waypointPath() const
{
QVariantList list;
for ( auto wp : _waypoints)
list.append(QVariant::fromValue(wp));
return list;
}
//QStringList WimaController::loadNameFilters() const //QStringList WimaController::loadNameFilters() const
//{ //{
// QStringList filters; // QStringList filters;
...@@ -376,66 +384,6 @@ bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoo ...@@ -376,66 +384,6 @@ bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoo
return retVal; return retVal;
} }
bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList)
{
return extractCoordinateList(missionItems, coordinateList, 0, missionItems.count()-1);
}
bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList, int startIndex, int endIndex)
{
if ( startIndex >= 0
&& startIndex < missionItems.count()
&& endIndex >= 0
&& endIndex < missionItems.count()) {
if (startIndex > endIndex) {
if (!extractCoordinateList(missionItems, coordinateList, startIndex, missionItems.count()-1))
return false;
if (!extractCoordinateList(missionItems, coordinateList, 0, endIndex))
return false;
} else {
for (int i = startIndex; i <= endIndex; i++) {
SimpleMissionItem *mItem = missionItems.value<SimpleMissionItem *>(i);
if (mItem == nullptr) {
coordinateList.clear();
return false;
}
coordinateList.append(mItem->coordinate());
}
}
} else
return false;
return true;
}
bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList)
{
return extractCoordinateList(missionItems, coordinateList, 0 , missionItems.count()-1);
}
bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex)
{
QVector<QGeoCoordinate> geoCoordintateList;
bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex);
if (!retValue)
return false;
for (int i = 0; i < geoCoordintateList.size(); i++) {
QGeoCoordinate vertex = geoCoordintateList[i];
if ( (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
|| !vertex.isValid())
geoCoordintateList.removeAt(i);
}
for (auto coordinate : geoCoordintateList)
coordinateList.append(QVariant::fromValue(coordinate));
return true;
}
/*! /*!
* \fn void WimaController::containerDataValidChanged(bool valid) * \fn void WimaController::containerDataValidChanged(bool valid)
* Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true). * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true).
...@@ -451,7 +399,7 @@ bool WimaController::_fetchContainerData() ...@@ -451,7 +399,7 @@ bool WimaController::_fetchContainerData()
_visualItems.clear(); _visualItems.clear();
_missionItems.clearAndDeleteContents(); _missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents(); _currentMissionItems.clearAndDeleteContents();
_waypointPath.clear(); _waypoints.clear();
_currentWaypointPath.clear(); _currentWaypointPath.clear();
_snakeTiles.polygons().clear(); _snakeTiles.polygons().clear();
_snakeTilesLocal.polygons().clear(); _snakeTilesLocal.polygons().clear();
...@@ -675,12 +623,12 @@ bool WimaController::_calcNextPhase() ...@@ -675,12 +623,12 @@ bool WimaController::_calcNextPhase()
QVector<QGeoCoordinate> CSWpList; // list with potential waypoints (from _missionItems), for _currentMissionItems QVector<QGeoCoordinate> CSWpList; // list with potential waypoints (from _missionItems), for _currentMissionItems
if (!reverse) { if (!reverse) {
if (!extractCoordinateList(_missionItems, CSWpList, _startWaypointIndex, _endWaypointIndex)) { if (!getCoordinates(_missionItems, CSWpList, _startWaypointIndex, _endWaypointIndex)) {
qWarning("WimaController::calcNextPhase(): error on waypoint extraction."); qWarning("WimaController::calcNextPhase(): error on waypoint extraction.");
return false; return false;
} }
} else { } else {
if (!extractCoordinateList(_missionItems, CSWpList, _endWaypointIndex, _startWaypointIndex)) { if (!getCoordinates(_missionItems, CSWpList, _endWaypointIndex, _startWaypointIndex)) {
qWarning("WimaController::calcNextPhase(): error on waypoint extraction."); qWarning("WimaController::calcNextPhase(): error on waypoint extraction.");
return false; return false;
} }
...@@ -859,15 +807,19 @@ bool WimaController::_calcNextPhase() ...@@ -859,15 +807,19 @@ bool WimaController::_calcNextPhase()
void WimaController::_updateWaypointPath() void WimaController::_updateWaypointPath()
{ {
_waypointPath.clear(); _waypoints.clear();
extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1); getCoordinates(_missionItems,
_waypoints,
0,
std::size_t( std::max(_missionItems.count()-1,0) )
);
emit waypointPathChanged(); emit waypointPathChanged();
} }
void WimaController::_updateCurrentPath() void WimaController::_updateCurrentPath()
{ {
_currentWaypointPath.clear(); _currentWaypointPath.clear();
extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1); getCoordinates(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1);
emit currentWaypointPathChanged(); emit currentWaypointPathChanged();
} }
...@@ -1408,7 +1360,7 @@ void WimaController::_saveCurrentMissionItemsToBuffer() ...@@ -1408,7 +1360,7 @@ void WimaController::_saveCurrentMissionItemsToBuffer()
} }
void WimaController::_progressFromJson(JsonDocUPtr pDoc, void WimaController::_progressFromJson(JsonDocUPtr pDoc,
QNemoProgress &progress) QNemoProgress &progress)
{ {
int requiredSize = _snakeTilesLocal.polygons().size(); int requiredSize = _snakeTilesLocal.polygons().size();
if ( !_pRosBridge->casePacker()->unpack(pDoc, progress) if ( !_pRosBridge->casePacker()->unpack(pDoc, progress)
...@@ -1418,3 +1370,10 @@ void WimaController::_progressFromJson(JsonDocUPtr pDoc, ...@@ -1418,3 +1370,10 @@ void WimaController::_progressFromJson(JsonDocUPtr pDoc,
emit WimaController::nemoProgressChanged(); emit WimaController::nemoProgressChanged();
} }
template<>
QVariant getCoordinate<QVariant>(const SimpleMissionItem* item)
{
return QVariant::fromValue(item->coordinate());
}
...@@ -116,7 +116,7 @@ public: ...@@ -116,7 +116,7 @@ public:
WimaDataContainer* dataContainer (void) { return _container; } WimaDataContainer* dataContainer (void) { return _container; }
QmlObjectListModel* missionItems (void) { return &_missionItems; } QmlObjectListModel* missionItems (void) { return &_missionItems; }
QmlObjectListModel* currentMissionItems (void) { return &_currentMissionItems; } QmlObjectListModel* currentMissionItems (void) { return &_currentMissionItems; }
QVariantList waypointPath (void) { return _waypointPath; } QVariantList waypointPath (void) const;
QVariantList currentWaypointPath (void) { return _currentWaypointPath; } QVariantList currentWaypointPath (void) { return _currentWaypointPath; }
Fact* enableWimaController (void) { return &_enableWimaController; } Fact* enableWimaController (void) { return &_enableWimaController; }
Fact* overlapWaypoints (void) { return &_overlapWaypoints; } Fact* overlapWaypoints (void) { return &_overlapWaypoints; }
...@@ -200,14 +200,6 @@ public: ...@@ -200,14 +200,6 @@ public:
QJsonDocument saveToJson(FileType fileType); QJsonDocument saveToJson(FileType fileType);
bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path); bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path);
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList
bool extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList);
/// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList.
bool extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList, int startIndex, int endIndex);
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList
bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList);
/// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList.
bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex);
signals: signals:
void masterControllerChanged (void); void masterControllerChanged (void);
...@@ -285,7 +277,7 @@ private: ...@@ -285,7 +277,7 @@ private:
QmlObjectListModel _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems, QmlObjectListModel _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems,
// _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage // _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage
QmlObjectListModel _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked QmlObjectListModel _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked
QVariantList _waypointPath; // path connecting the items in _missionItems QVector<QGeoCoordinate> _waypoints; // path connecting the items in _missionItems
QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems
QGeoCoordinate _takeoffLandPostion; QGeoCoordinate _takeoffLandPostion;
...@@ -376,3 +368,53 @@ private: ...@@ -376,3 +368,53 @@ private:
*/ */
template<class CoordinateType>
CoordinateType getCoordinate(const SimpleMissionItem* item){
return item->coordinate();
}
template<>
QVariant getCoordinate<QVariant>(const SimpleMissionItem* item);
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool getCoordinates(QmlObjectListModel &missionItems,
ContainerType<CoordinateType> &coordinateList,
std::size_t startIndex,
std::size_t endIndex){
if ( startIndex < std::size_t(missionItems.count())
&& endIndex < std::size_t(missionItems.count()) ) {
if (startIndex > endIndex) {
if (!getCoordinates(missionItems, coordinateList, startIndex, missionItems.count()-1))
return false;
if (!getCoordinates(missionItems, coordinateList, 0, endIndex))
return false;
} else {
for (std::size_t i = startIndex; i <= endIndex; i++) {
SimpleMissionItem *mItem = missionItems.value<SimpleMissionItem *>(int(i));
if (mItem == nullptr) {
coordinateList.clear();
return false;
}
coordinateList.append(getCoordinate<CoordinateType>(mItem));
}
}
} else
return false;
return true;
}
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool getCoordinates(QmlObjectListModel &missionItems,
ContainerType<CoordinateType> &coordinateList){
return getCoordinates(missionItems, coordinateList, 0, missionItems.count());
}
#include "WimaVehicle.h"
WimaVehicle::WimaVehicle(QObject *parent):
QObject (parent)
,_vehicle (nullptr)
,_serviceArea (nullptr)
,_vehicleCorridor (nullptr)
,_operationArea (nullptr)
{
}
void WimaVehicle::setVehicle(Vehicle *vehicle)
{
if(vehicle != nullptr){
_vehicle = vehicle;
}else {
qWarning("Not a valid vehicle!");
}
}
void WimaVehicle::setServiceArea(WimaServiceArea *servicePolygon)
{
if(servicePolygon != nullptr){
_serviceArea = servicePolygon;
}else{
qWarning("Not a valid service Polygon!");
}
}
void WimaVehicle::setVCorridor(WimaVCorridor *vehicleCorridor)
{
if(vehicleCorridor != nullptr){
_vehicleCorridor = vehicleCorridor;
}else{
qWarning("Not a valid vehicle Corridor!");
}
}
void WimaVehicle::setOperationArea(WimaGOperationArea *operationArea)
{
if(operationArea != nullptr){
_operationArea = operationArea;
}else{
qWarning("Not a valid measurementPolygon!");
}
}
#pragma once
#include <QObject>
#include "Vehicle.h"
class WimaServiceArea;
class WimaVCorridor;
class WimaGOperationArea;
class WimaVehicle : public QObject
{
Q_OBJECT
public:
WimaVehicle(QObject* parent = nullptr);
WimaServiceArea* serviceArea (void) const { return _serviceArea;}
WimaVCorridor* vehicleCorridor (void) const { return _vehicleCorridor;}
WimaGOperationArea* operationArea (void) const { return _operationArea;}
void setVehicle (Vehicle* vehicle);
void setServiceArea (WimaServiceArea* serviceArea);
void setVCorridor (WimaVCorridor* vehicleCorridor);
void setOperationArea (WimaGOperationArea* operationArea);
private:
Vehicle* _vehicle;
WimaServiceArea* _serviceArea;
WimaVCorridor* _vehicleCorridor;
WimaGOperationArea* _operationArea;
};
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QDOCINDEX>
<INDEX url="" title="" version="" project="">
<namespace threadsafety="unspecified" name="" status="active" access="public"/>
</INDEX>
This source diff could not be displayed because it is too large. You can view the blob instead.
How to exchange mission between WimaPlaner and WimaController.
1) convert mission into waypoints
2) send waypoints to WimaController
3) upload desired subset of waypoints to vehicle
4) start/continue mission
5) return vehicle on battery low
6) go to 4) or quit if last waypoint reached
investigate load, loadFromVehicle, syncInProgress, _newMissionItemsAvailableFromVehicle
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment