Skip to content

Q
qgroundcontrol
Commit 99e5ae38 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

addapting wima stuff to qgc master branch, code not ready

parent 5e177398
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 320 additions and 702 deletions
src/MissionManager/SimpleMissionItem.h
View file @ 99e5ae38
......@@ -118,7 +118,7 @@ public:
QString mapVisualQML (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
ReadyForSaveState readyForSaveState (void) const final;
double additionalTimeDelay (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return true; }
......
src/MissionManager/TransectStyleComplexItem.h
View file @ 99e5ae38
......@@ -88,8 +88,8 @@ public:
// Overrides from VisualMissionItem
void save (QJsonArray& planItems) override = 0;
bool specifiesCoordinate (void) const override = 0;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
virtual void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
virtual void applyNewAltitude (double newAltitude) final;
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
......@@ -100,7 +100,7 @@ public:
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
ReadyForSaveState readyForSaveState (void) const override;
QString commandDescription (void) const override { return tr("Transect"); }
QString commandName (void) const override { return tr("Transect"); }
......
src/Wima/CircularSurvey.cc
View file @ 99e5ae38
......@@ -19,6 +19,8 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")
template <typename T>
......@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::variantName = "Variant";
CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
CircularSurvey::CircularSurvey(PlanMasterController *masterController, bool flyView,
const QString &kmlOrShpFile, QObject *parent)
: TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent),
: TransectStyleComplexItem(masterController, flyView, settingsGroup, parent),
_state(STATE::IDLE),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
......@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&CircularSurvey::_setTransects);
connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this,
&CircularSurvey::calculatingChanged);
this->_transectsDirty = true;
// Altitude
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::coordinateHasRelativeAltitudeChanged);
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::exitCoordinateHasRelativeAltitudeChanged);
// Register Generators.
auto lg = std::make_shared<routing::LinearGenerator>(this->_pAreaData);
......@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return false;
}
if (!_load(complexObject, errorString)) {
if (!load(complexObject, sequenceNumber, errorString)) {
_ignoreRecalc = false;
return false;
}
......@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_ignoreRecalc = false;
_recalcComplexDistance();
if (_cameraShots == 0) {
// Shot count was possibly not available from plan file
_recalcCameraShots();
......@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
bool CircularSurvey::specifiesCoordinate() const { return true; }
void CircularSurvey::appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty)
return;
if (_loadedMissionItems.count()) {
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems(items, missionItemParent);
} else {
// Build the mission items on the fly
_buildAndAppendMissionItems(items, missionItemParent);
}
}
void CircularSurvey::_appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty)
return;
int seqNum = _sequenceNumber;
for (const MissionItem *loadedMissionItem : _loadedMissionItems) {
MissionItem *item = new MissionItem(*loadedMissionItem, missionItemParent);
item->setSequenceNumber(seqNum++);
items.append(item);
}
}
void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty || _transects.count() == 0)
return;
MissionItem *item;
int seqNum = _sequenceNumber;
MAV_FRAME mavFrame =
followTerrain() || !_cameraCalc.distanceToSurfaceRelative()
? MAV_FRAME_GLOBAL
: MAV_FRAME_GLOBAL_RELATIVE_ALT;
for (const QList<TransectStyleComplexItem::CoordInfo_t> &transect :
_transects) {
// bool transectEntry = true;
for (const CoordInfo_t &transectCoordInfo : transect) {
item = new MissionItem(
seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame,
0, // Hold time (delay for hover and capture to settle vehicle
// before image is taken)
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
transectCoordInfo.coord.latitude(),
transectCoordInfo.coord.longitude(),
transectCoordInfo.coord.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
}
}
bool CircularSurvey::_switchToGenerator(
const CircularSurvey::PtrGenerator &newG) {
if (this->_pGenerator != newG) {
......@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
}
void CircularSurvey::_updateWorker() {
// Mark transects as dirty.
this->_transectsDirty = true;
// Reset data.
this->_transects.clear();
this->_variantVector.clear();
......@@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() {
}
}
void CircularSurvey::applyNewAltitude(double newAltitude) {
_cameraCalc.valueSetIsDistance()->setRawValue(true);
_cameraCalc.distanceToSurface()->setRawValue(newAltitude);
_cameraCalc.setDistanceToSurfaceRelative(true);
}
double CircularSurvey::timeBetweenShots() { return 1; }
QString CircularSurvey::commandDescription() const {
......@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString CircularSurvey::abbreviation() const { return tr("C.S."); }
bool CircularSurvey::readyForSave() const {
return TransectStyleComplexItem::readyForSave() && !_transectsDirty &&
this->_state == STATE::IDLE;
TransectStyleComplexItem::ReadyForSaveState
CircularSurvey::readyForSaveState() const {
if (TransectStyleComplexItem::readyForSaveState() ==
TransectStyleComplexItem::ReadyForSaveState::ReadyForSave) {
if (this->_state == STATE::IDLE) {
return ReadyForSaveState::ReadyForSave;
} else {
return ReadyForSaveState::NotReadyForSaveData;
}
} else {
return TransectStyleComplexItem::readyForSaveState();
}
}
double CircularSurvey::additionalTimeDelay() const { return 0; }
......@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
<< " ms";
}
void CircularSurvey::_recalcComplexDistance() {
_complexDistance = 0;
if (_transectsDirty)
return;
for (int i = 0; i < _visualTransectPoints.count() - 1; i++) {
_complexDistance +=
_visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(
_visualTransectPoints[i + 1].value<QGeoCoordinate>());
}
emit complexDistanceChanged();
}
// no cameraShots in Circular Survey, add if desired
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
......@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
&CircularSurvey::_changeVariant);
this->_changeVariantWorker();
// Mark transect ready.
this->_transectsDirty = false;
this->_state = STATE::SKIPP;
this->_rebuildTransects();
} else {
......
src/Wima/CircularSurvey.h
View file @ 99e5ae38
......@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
using PtrGenerator = std::shared_ptr<routing::GeneratorBase>;
using PtrRoutingData = std::shared_ptr<RoutingData>;
using PtrWorker = std::shared_ptr<RoutingThread>;
using PtrWorker = std::unique_ptr<RoutingThread>;
using Transects = QList<QList<CoordInfo_t>>;
using Variant = Transects;
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default
/// polygon
CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile,
CircularSurvey(PlanMasterController *masterController, bool flyView, const QString &kmlOrShpFile,
QObject *parent);
~CircularSurvey();
......@@ -63,14 +60,11 @@ public:
QString mapVisualQML(void) const override final;
void save(QJsonArray &planItems) override final;
bool specifiesCoordinate(void) const override final;
void appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) override final;
void applyNewAltitude(double newAltitude) override final;
double timeBetweenShots(void) override final;
QString commandDescription(void) const override final;
QString commandName(void) const override final;
QString abbreviation(void) const override final;
bool readyForSave(void) const override final;
ReadyForSaveState readyForSaveState(void) const override final;
double additionalTimeDelay(void) const override final;
// Generator
......@@ -97,7 +91,6 @@ signals:
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final;
// Worker functions.
......@@ -108,11 +101,6 @@ private slots:
void _reverseWorker();
private:
void _appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
bool _switchToGenerator(const PtrGenerator &newG);
// State.
......@@ -139,7 +127,6 @@ private:
PtrGenerator _pGenerator;
// Routing.
using Variant = Transects;
QVector<Variant> _variantVector;
PtrWorker _pWorker;
};
src/Wima/Geometry/WimaArea.cc
View file @ 99e5ae38
......@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
} else if (retValue == JoinPolygonError::PathSizeLow) {
qWarning("Polygon vertex count is low.");
} else {
QVector<QGeoCoordinate> path;
QList<QGeoCoordinate> path;
toGeoList(joinedPolygon, origin, path);
// qWarning("after transform");
// qWarning() << path;
......@@ -491,38 +491,6 @@ void WimaArea::init() {
&WimaArea::recalcInteractivity);
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the console.
*/
void print(const WimaArea &area) {
QString message;
print(area, message);
qWarning() << message;
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the \a outputString.
*/
void print(const WimaArea &area, QString &outputString) {
outputString.append(QString("Type: %1\n").arg(area.objectName()));
print(static_cast<const QGCMapPolygon &>(area), outputString);
outputString.append(QString("Maximum Altitude: %1\n").arg(area._maxAltitude));
outputString.append(
QString("Border Polygon Offset: %1\n")
.arg(area._borderPolygonOffset.rawValue().toDouble()));
outputString.append(
QString("Border Polygon Coordinates\n")
.arg(area._borderPolygonOffset.rawValue().toDouble()));
for (int i = 0; i < area._borderPolygon.count(); i++) {
QGeoCoordinate coordinate = area._borderPolygon.vertexCoordinate(i);
outputString.append(
QString("%1\n").arg(coordinate.toString(QGeoCoordinate::Degrees)));
}
}
// QDoc Documentation
/*!
......
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ 99e5ae38
......@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData &TileData::operator=(const TileData &other) {
this->tiles.clearAndDeleteContents();
for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) {
const auto *obj = other.tiles.get(i);
const auto *obj = other.tiles[i];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
......@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
}
bool TileData::operator==(const TileData &other) const {
return this->tiles == other.tiles &&
this->tileCenterPoints == other.tileCenterPoints;
if (this->tileCenterPoints == other.tileCenterPoints &&
this->tiles.count() == other.tiles.count()) {
for (int i = 0; i < other.tiles.count(); ++i) {
if (this->tiles[i] != other.tiles[i]) {
return false;
}
}
return true;
} else {
return false;
}
}
bool TileData::operator!=(const TileData &other) const {
......@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) {
if (ready()) {
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble();
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
} else {
qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing.";
}
......@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
auto start = std::chrono::high_resolution_clock::now();
if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) {
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea;
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea;
// Check some conditions.
if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) {
this->count() >= 3 && this->isSimplePolygon()) {
setState(STATE::UPDATEING);
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto minArea =
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto minArea =
this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea;
auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
}
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
}
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
}
}
pData->moveToThread(th);
pData->moveToThread(th);
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
return pData;
}); // QtConcurrent::run()
return pData;
}); // QtConcurrent::run()
this->_watcher.setFuture(future);
}
this->_watcher.setFuture(future);
}
}
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): execution time: "
......
src/Wima/Geometry/WimaServiceArea.cc
View file @ 99e5ae38
#include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(WimaServiceAreaLog, "WimaServiceAreaLog")
const char *WimaServiceArea::wimaServiceAreaName = "Service Area";
const char *WimaServiceArea::depotLatitudeName = "DepotLatitude";
const char *WimaServiceArea::depotLongitudeName = "DepotLongitude";
......@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
} else if (this->_depot.isValid()) {
// Use nearest coordinate.
auto minDist = std::numeric_limits<double>::infinity();
auto minIt = this->pathReference().begin();
for (auto it = this->pathReference().begin();
it < this->pathReference().end(); ++it) {
const auto vertex = it->value<QGeoCoordinate>();
auto d = vertex.distanceTo(this->_depot);
if (d < minDist) {
minDist = d;
minIt = it;
int minIndex = 0;
for (int idx = 0; idx < this->pathModel().count(); ++idx) {
const QObject *obj = this->pathModel()[idx];
const auto *vertex = qobject_cast<const QGeoCoordinate *>(obj);
if (vertex != nullptr) {
auto d = vertex->distanceTo(this->_depot);
if (d < minDist) {
minDist = d;
minIndex = idx;
}
} else {
qCCritical(WimaServiceAreaLog) << "init(): nullptr catched!";
}
}
this->setDepot(minIt->value<QGeoCoordinate>());
} else if (this->pathReference().size() > 0) {
this->setDepot(*this->pathModel().value<QGeoCoordinate *>(minIndex));
} else if (this->pathModel().count() > 0) {
// Use first coordinate.
this->setDepot(this->pathReference().value(0).value<QGeoCoordinate>());
this->setDepot(*this->pathModel().value<QGeoCoordinate *>(0));
}
}
});
......
src/Wima/Snake/CircularGenerator.cpp
View file @ 99e5ae38
......@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
const auto *tile = tiles->value<const SnakeTile *>(i);
const auto *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
src/Wima/Snake/LinearGenerator.cpp
View file @ 99e5ae38
......@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
const auto *tile = tiles->value<const SnakeTile *>(i);
const QObject *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
src/Wima/Snake/NemoInterface.cpp
View file @ 99e5ae38
......@@ -97,7 +97,7 @@ private:
};
using StatusMap = std::map<NemoInterface::STATUS, QString>;
StatusMap statusMap{
static StatusMap statusMap{
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
std::make_pair<NemoInterface::STATUS, QString>(
......@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto connectionString = connectionStringFact->rawValue().toString();
if (ros_bridge::isValidConnectionString(
connectionString.toLocal8Bit().data())) {
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
} else {
QString defaultString("localhost:9090");
qgcApp()->showMessage("ROS Bridge connection string invalid: " +
connectionString);
qgcApp()->showMessage("Resetting connection string to: " + defaultString);
connectionStringFact->setRawValue(
QVariant(defaultString)); // calls this function recursively
qgcApp()->warningMessageBoxOnMainThread(
"Nemo Interface",
"Websocket connection string possibly invalid: " + connectionString +
". Trying to connect anyways.");
}
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
......@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
const auto *obj = tileData.tiles.get(0);
const auto *obj = tileData.tiles[0];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
if (tile->coordinateList().size() > 0) {
if (tile->coordinateList().first().isValid()) {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles.get(i);
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles[i];
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
break;
break;
}
}
}
} else {
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
}
}
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
}
}
}
} else {
this->tileData.clear();
......@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
progressMsg.progress().size() !=
requiredSize) { // Some error occured.
progressMsg.progress().clear();
qgcApp()->showMessage("Invalid progress message received.");
qgcApp()->informationMessageBoxOnMainThread(
"Nemo Interface", "Invalid progress message received.");
}
emit this->parent->progressChanged();
......@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
......@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
......@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
this->doTopicServiceSetup();
this->topicServiceSetupDone = true;
this->topicServiceSetupDone = true;
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else if (this->pRosBridge->isRunning() &&
......@@ -395,7 +394,7 @@ void NemoInterface::Impl::publishTilesENU() {
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishTilesENU: could not create json document.";
......@@ -408,7 +407,7 @@ void NemoInterface::Impl::publishENUOrigin() {
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
jOrigin->GetAllocator())) {
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishENUOrigin: could not create json document.";
......
src/Wima/WaypointManager/DefaultManager.cpp
View file @ 99e5ae38
......@@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem(
using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->vehicle(), _settings->isFlyView(),
_settings->masterController(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) {
qWarning(
"WaypointManager::DefaultManager::next(): insertMissionItem failed.");
......@@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() {
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(),
initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView());
// Calculate path from home to first waypoint.
......@@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() {
Q_ASSERT(takeoffItem != nullptr);
return false;
}
makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle());
makeTakeOffCmd(takeoffItem, _settings->masterController());
// Create change speed item.
_insertMissionItem(_settings->homePosition(), 2 /*insertion index*/,
......@@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
makeLandCmd(landItem, _settings->masterController());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
......
src/Wima/WaypointManager/RTLManager.cpp
View file @ 99e5ae38
......@@ -88,7 +88,7 @@ bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->vehicle(), _settings->isFlyView(),
_settings->masterController(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) {
qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed.");
Q_ASSERT(false);
......@@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() {
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(),
initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView());
// Calculate path from vehicle to home position.
......@@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() {
qWarning("WaypointManager::RTLManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
makeLandCmd(landItem, _settings->masterController());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
......
src/Wima/WaypointManager/Utils.cpp
View file @ 99e5ae38
#include "Utils.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include <iostream>
#include <QGeoCoordinate>
template<>
QVariant WaypointManager::Utils::getCoordinate(const SimpleMissionItem* item)
{
return QVariant::fromValue(item->coordinate());
}
bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
long index,
QmlObjectListModel &list,
Vehicle* vehicle,
PlanMasterController* masterController,
bool flyView,
QObject *parent,
bool doUpdates)
......@@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
} else {
if (parent == nullptr)
parent = &list;
SimpleMissionItem * newItem = new SimpleMissionItem(vehicle, flyView, parent);
auto *newItem = new SimpleMissionItem(masterController, flyView, MissionItem(), parent);
int sequenceNumber = detail::nextSequenceNumber(list);
newItem->setSequenceNumber(sequenceNumber);
......@@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list)
}
void WaypointManager::Utils::initialize(QmlObjectListModel &list,
Vehicle *vehicle,
PlanMasterController *masterController,
bool flyView)
{
MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, flyView, &list);
MissionSettingsItem* settingsItem = new MissionSettingsItem(masterController, flyView, &list);
list.insert(0, settingsItem);
}
......@@ -189,26 +185,6 @@ void WaypointManager::Utils::printList(QmlObjectListModel &list)
}
}
bool WaypointManager::Utils::makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle)
{
if ( vehicle->fixedWing()
|| vehicle->vtol()
|| vehicle->multiRotor()
)
{
MAV_CMD takeoffCmd = vehicle->vtol()
? MAV_CMD_NAV_VTOL_TAKEOFF
: MAV_CMD_NAV_TAKEOFF;
if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
auto c = item->coordinate();
item->setCommand(takeoffCmd);
item->setCoordinate(c);
return true;
}
}
return false;
}
void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
{
assert(speed > 0);
......@@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
item->setCoordinate(c);
item->missionItem().setParam2(speed);
}
bool WaypointManager::Utils::makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle)
{
MAV_CMD landCmd = vehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
item->setCommand(landCmd);
} else {
Q_ASSERT(false);
qWarning("WaypointManager::Utils::makeLandCmd(): Land command not supported!");
return false;
}
return true;
}
src/Wima/WaypointManager/Utils.h
View file @ 99e5ae38
This diff is collapsed.
src/Wima/WimaController.cc
View file @ 99e5ae38
......@@ -33,8 +33,6 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value);
}
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
const char *WimaController::areaItemsName = "AreaItems";
......@@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent)
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)),
_enableWimaController(settingsGroup,
_metaDataMap[enableWimaControllerName]),
_flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]),
_altitude(settingsGroup, _metaDataMap[altitudeName]),
_lowBatteryHandlingTriggered(false),
_batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {
// Set up facts for waypoint manager.
connect(&_flightSpeed, &Fact::rawValueChanged, this,
&WimaController::_updateflightSpeed);
connect(&_altitude, &Fact::rawValueChanged, this,
&WimaController::_updateAltitude);
// Periodic.
connect(&_eventTimer, &QTimer::timeout, this,
&WimaController::_eventTimerHandler);
_eventTimer.start(EVENT_TIMER_INTERVAL);
_metaDataMap[enableWimaControllerName]){
// PlanData and Progress.
connect(WimaBridge::instance(), &WimaBridge::planDataChanged, this,
&WimaController::planDataChangedHandler);
......@@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) {
emit missionControllerChanged();
}
void WimaController::requestSmartRTL() {
qCWarning(WimaControllerLog) << "requestSmartRTL() called";
QString errorString("Smart RTL requested.");
if (!_SRTLPrecondition(errorString)) {
qgcApp()->showMessage(errorString);
return;
}
emit smartRTLRequestConfirm();
}
bool WimaController::upload() {
auto &items = _currentWM->currentMissionItems();
if (_masterController && _masterController->managerVehicle() &&
......@@ -141,68 +113,6 @@ bool WimaController::upload() {
}
}
bool WimaController::forceUpload() {
auto &currentMissionItems = _currentWM->currentMissionItems();
if (currentMissionItems.count() < 1 || !_missionController ||
!_masterController) {
qCWarning(WimaControllerLog)
<< "forceUpload(): error:"
<< "currentMissionItems.count(): " << currentMissionItems.count()
<< "_missionController: " << _missionController
<< "_masterController: " << _masterController;
return false;
} else {
_missionController->removeAll();
// Set homeposition of settingsItem.
QmlObjectListModel *visuals = _missionController->visualItems();
MissionSettingsItem *settingsItem =
visuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qCWarning(WimaControllerLog) << "updateCurrentMissionItems(): nullptr";
return false;
} else {
settingsItem->setCoordinate(_WMSettings.homePosition());
// Copy mission items to _missionController and send them.
for (int i = 1; i < currentMissionItems.count(); i++) {
auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
_missionController->insertSimpleMissionItem(*item, visuals->count());
}
_masterController->sendToVehicle();
return true;
}
}
}
void WimaController::removeFromVehicle() {
if (_masterController && _missionController) {
_masterController->removeAllFromVehicle();
_missionController->removeAll();
}
}
void WimaController::executeSmartRTL() {
qCWarning(WimaControllerLog) << "executeSmartRTL() called";
if (_masterController && _masterController->managerVehicle()) {
forceUpload();
_masterController->managerVehicle()->startMission();
}
}
void WimaController::initSmartRTL() {
qCWarning(WimaControllerLog) << "initSmartRTL() called";
_initSmartRTL();
}
void WimaController::removeVehicleTrajectoryHistory() {
if (_masterController && _masterController->managerVehicle()) {
_masterController->managerVehicle()
->trajectoryPoints()
->clearAndDeleteContents();
}
}
bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path) {
......@@ -312,109 +222,3 @@ void WimaController::_updateAltitude() {
emit missionItemsChanged();
emit waypointPathChanged();
}
void WimaController::_checkBatteryLevel() {
if (_missionController && _masterController &&
_masterController->managerVehicle()) {
Vehicle *managerVehicle = masterController()->managerVehicle();
WimaSettings *wimaSettings =
qgcApp()->toolbox()->settingsManager()->wimaSettings();
int threshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
bool enabled = _enableWimaController.rawValue().toBool();
unsigned int minTime =
wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
if (enabled) {
Fact *battery1percentRemaining =
managerVehicle->battery1FactGroup()->getFact(
VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining =
managerVehicle->battery2FactGroup()->getFact(
VehicleBatteryFactGroup::_percentRemainingFactName);
if (battery1percentRemaining->rawValue().toDouble() < threshold &&
battery2percentRemaining->rawValue().toDouble() < threshold) {
if (!_lowBatteryHandlingTriggered) {
_lowBatteryHandlingTriggered = true;
if (!(_missionController->remainingTime() <= minTime)) {
requestSmartRTL();
}
}
} else {
_lowBatteryHandlingTriggered = false;
}
}
}
}
void WimaController::_eventTimerHandler() {
// Battery level check necessary?
Fact *enableLowBatteryHandling = qgcApp()
->toolbox()
->settingsManager()
->wimaSettings()
->enableLowBatteryHandling();
if (enableLowBatteryHandling->rawValue().toBool() &&
_batteryLevelTicker.ready())
_checkBatteryLevel();
}
void WimaController::_smartRTLCleanUp(bool flying) {
if (!flying && _missionController) { // vehicle has landed
_switchWaypointManager(_emptyWM);
_missionController->removeAll();
disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged,
this, &WimaController::_smartRTLCleanUp);
}
}
bool WimaController::_SRTLPrecondition(QString &errorString) {
if (!_planDataValid) {
errorString.append(tr("No WiMA data available. Please define at least a "
"measurement and a service area."));
return false;
}
return _rtlWM.checkPrecondition(errorString);
}
void WimaController::_switchWaypointManager(
WaypointManager::ManagerBase &manager) {
if (_currentWM != &manager) {
_currentWM = &manager;
emit missionItemsChanged();
emit waypointPathChanged();
qCWarning(WimaControllerLog) << "_switchWaypointManager: statistics update "
"missing.";
}
}
void WimaController::_initSmartRTL() {
static int attemptCounter = 0;
attemptCounter++;
QString errorString;
if (_SRTLPrecondition(errorString)) {
if (_missionController && _masterController &&
_masterController->managerVehicle()) {
_masterController->managerVehicle()->pauseVehicle();
connect(_masterController->managerVehicle(), &Vehicle::flyingChanged,
this, &WimaController::_smartRTLCleanUp);
if (_rtlWM.update()) { // Calculate return path.
_switchWaypointManager(_rtlWM);
removeFromVehicle();
attemptCounter = 0;
emit smartRTLPathConfirm();
}
}
} else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
errorString.append(
tr("Smart RTL: No success after maximum number of attempts."));
qgcApp()->showMessage(errorString);
attemptCounter = 0;
} else {
_smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this,
&WimaController::_initSmartRTL);
}
}
src/Wima/WimaController.h
View file @ 99e5ae38
......@@ -70,10 +70,6 @@ public:
// Member Methodes
Q_INVOKABLE WimaController *thisPointer();
// Smart RTL.
Q_INVOKABLE void requestSmartRTL();
Q_INVOKABLE void initSmartRTL();
Q_INVOKABLE void executeSmartRTL();
// Other.
Q_INVOKABLE void removeVehicleTrajectoryHistory();
Q_INVOKABLE bool upload();
......@@ -97,9 +93,6 @@ signals:
// Waypoints.
void missionItemsChanged(void);
void waypointPathChanged(void);
// Smart RTL.
void smartRTLRequestConfirm(void);
void smartRTLPathConfirm(void);
// Upload.
void forceUploadConfirm(void);
......@@ -111,11 +104,6 @@ private slots:
QVector<QGeoCoordinate> &path);
void _updateflightSpeed(void);
void _updateAltitude(void);
// SMART RTL
void _checkBatteryLevel(void);
bool _SRTLPrecondition(QString &errorString);
void _initSmartRTL();
void _smartRTLCleanUp(bool flying);
// Snake.
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
// Periodic tasks.
......@@ -152,10 +140,6 @@ private:
SettingsFact _flightSpeed; // mission flight speed
SettingsFact _altitude; // mission altitude
// Smart RTL.
QTimer _smartRTLTimer;
bool _lowBatteryHandlingTriggered;
// Periodic tasks.
QTimer _eventTimer;
EventTicker _batteryLevelTicker;
......
src/Wima/WimaPlaner.cc
View file @ 99e5ae38
This diff is collapsed.
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