Skip to content

Q
qgroundcontrol
Commit 53fdc29e authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

.old files removed

parent 5f0f205b
Hide whitespace changes
Inline Side-by-side
Showing with 0 additions and 2208 deletions
src/Wima/Geometry/WimaMeasurementArea.cc.orig deleted 100644 → 0
View file @ 5f0f205b
#include "WimaMeasurementArea.h"
#include "QtConcurrentRun"
#include "SnakeTile.h"
#include "snake.h"
#include <boost/units/systems/si.hpp>
#include "QGCLoggingCategory.h"
#ifndef SNAKE_MAX_TILES
#define SNAKE_MAX_TILES 1000
#endif
QGC_LOGGING_CATEGORY(WimaMeasurementAreaLog, "WimaMeasurementAreaLog")
TileData::TileData() : tiles(this) {}
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 *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
} else {
qCWarning(WimaMeasurementAreaLog) << "TileData::operator=: nullptr";
}
}
this->tileCenterPoints = other.tileCenterPoints;
return *this;
}
bool TileData::operator==(const TileData &other) const {
return this->tiles == other.tiles &&
this->tileCenterPoints == other.tileCenterPoints;
}
bool TileData::operator!=(const TileData &other) const {
return !this->operator==(other);
}
void TileData::clear() {
this->tiles.clearAndDeleteContents();
this->tileCenterPoints.clear();
}
size_t TileData::size() const {
if (tiles.count() == tileCenterPoints.size()) {
return tiles.count();
} else {
return 0;
}
}
const char *WimaMeasurementArea::settingsGroup = "MeasurementArea";
const char *WimaMeasurementArea::tileHeightName = "TileHeight";
const char *WimaMeasurementArea::tileWidthName = "TileWidth";
<<<<<<< HEAD
const char *WimaMeasurementArea::minTileAreaName = "MinTileAreaPercent";
=======
const char *WimaMeasurementArea::minTileAreaName = "MinTileArea";
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
const char *WimaMeasurementArea::showTilesName = "ShowTiles";
const char *WimaMeasurementArea::WimaMeasurementAreaName = "Measurement Area";
WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
: WimaArea(parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"),
this /* QObject parent */)),
_tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightName],
this /* QObject parent */)),
_tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName],
this /* QObject parent */)),
<<<<<<< HEAD
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
=======
_minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName],
this /* QObject parent */)),
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
init();
}
WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
QObject *parent)
: WimaArea(other, parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"),
this /* QObject parent */)),
_tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightName],
this /* QObject parent */)),
_tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName],
this /* QObject parent */)),
<<<<<<< HEAD
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
=======
_minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName],
this /* QObject parent */)),
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
init();
}
/*!
* \overload operator=()
*
* Calls the inherited operator WimaArea::operator=().
*/
WimaMeasurementArea &WimaMeasurementArea::
operator=(const WimaMeasurementArea &other) {
WimaArea::operator=(other);
return *this;
}
WimaMeasurementArea::~WimaMeasurementArea() {}
QString WimaMeasurementArea::mapVisualQML() const {
return QStringLiteral("WimaMeasurementAreaMapVisual.qml");
}
QString WimaMeasurementArea::editorQML() const {
return QStringLiteral("WimaMeasurementAreaEditor.qml");
}
Fact *WimaMeasurementArea::tileHeight() { return &_tileHeight; }
Fact *WimaMeasurementArea::tileWidth() { return &_tileWidth; }
<<<<<<< HEAD
Fact *WimaMeasurementArea::minTileArea() { return &_minTileAreaPercent; }
=======
Fact *WimaMeasurementArea::minTileArea() { return &_minTileArea; }
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
Fact *WimaMeasurementArea::showTiles() { return &_showTiles; }
QmlObjectListModel *WimaMeasurementArea::tiles() {
return &this->_tileData.tiles;
}
const QVector<int> &WimaMeasurementArea::progress() const {
return this->_progress;
}
QVector<int> WimaMeasurementArea::progressQml() const {
return this->_progress;
}
const QmlObjectListModel *WimaMeasurementArea::tiles() const {
return &this->_tileData.tiles;
}
const QVariantList &WimaMeasurementArea::tileCenterPoints() const {
return this->_tileData.tileCenterPoints;
}
const TileData &WimaMeasurementArea::tileData() const {
return this->_tileData;
}
int WimaMeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; }
bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) {
<<<<<<< HEAD
if (ready()) {
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;
} else {
qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing.";
}
=======
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileArea.rawValue().toDouble();
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
}
bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
QString &errorString) {
if (this->WimaArea::loadFromJson(json, errorString)) {
disableUpdate();
bool retVal = true;
if (!json.contains(tileHeightName) || !json[tileHeightName].isDouble()) {
errorString.append(tr("Could not load tile height!\n"));
retVal = false;
} else {
_tileHeight.setRawValue(json[tileHeightName].toDouble());
}
if (!json.contains(tileWidthName) || !json[tileWidthName].isDouble()) {
errorString.append(tr("Could not load tile width!\n"));
retVal = false;
} else {
_tileWidth.setRawValue(json[tileWidthName].toDouble());
}
<<<<<<< HEAD
if (!json.contains(minTileAreaName) || !json[minTileAreaName].isDouble()) {
errorString.append(tr("Could not load minimal tile area!\n"));
retVal = false;
} else {
_minTileAreaPercent.setRawValue(json[minTileAreaName].toDouble());
}
if (!json.contains(showTilesName) || !json[showTilesName].isBool()) {
=======
if (json.contains(showTilesName) && json[showTilesName].isBool()) {
_showTiles.setRawValue(json[showTilesName].toBool());
} else {
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
errorString.append(tr("Could not load show tiles !\n"));
retVal = false;
} else {
_showTiles.setRawValue(json[showTilesName].toBool());
}
enableUpdate();
doUpdate();
return retVal;
} else {
return false;
}
}
bool WimaMeasurementArea::setProgress(const QVector<int> &p) {
if (ready()) {
if (p.size() == this->tiles()->count() && this->_progress != p) {
this->_progress = p;
emit progressChanged();
emit progressAccepted();
return true;
}
}
return false;
}
//!
//! \brief WimaMeasurementArea::doUpdate
//! \pre WimaMeasurementArea::deferUpdate must be called first, don't call
//! this function directly!
void WimaMeasurementArea::doUpdate() {
using namespace snake;
using namespace boost::units;
auto start = std::chrono::high_resolution_clock::now();
// Check state.
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;
// Check some conditions.
if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) {
setState(STATE::UPDATEING);
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);
}
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);
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()
this->_watcher.setFuture(future);
}
}
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
}
void WimaMeasurementArea::deferUpdate() {
if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) {
qCDebug(WimaMeasurementAreaLog) << "defereUpdate(): defer update.";
if (this->_state == STATE::IDLE) {
this->_progress.clear();
this->_tileData.clear();
emit this->progressChanged();
emit this->tilesChanged();
}
this->setState(STATE::DEFERED);
this->_timer.start(100);
} else if (this->_state == STATE::UPDATEING) {
qCDebug(WimaMeasurementAreaLog) << "defereUpdate(): restart.";
setState(STATE::RESTARTING);
}
}
void WimaMeasurementArea::storeTiles() {
auto start = std::chrono::high_resolution_clock::now();
if (this->_state == STATE::UPDATEING) {
qCDebug(WimaMeasurementAreaLog) << "storeTiles(): update.";
this->_tileData = *this->_watcher.result();
// This is expensive. Drawing tiles is expensive too.
this->_progress = QVector<int>(this->_tileData.tiles.count(), 0);
this->progressChanged();
emit this->tilesChanged();
setState(STATE::IDLE);
} else if (this->_state == STATE::RESTARTING) {
qCDebug(WimaMeasurementAreaLog) << "storeTiles(): restart.";
doUpdate();
} else if (this->_state == STATE::STOP) {
qCDebug(WimaMeasurementAreaLog) << "storeTiles(): stop.";
}
qCDebug(WimaMeasurementAreaLog)
<< "storeTiles() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
}
void WimaMeasurementArea::disableUpdate() {
setState(STATE::IDLE);
this->_timer.stop();
}
void WimaMeasurementArea::enableUpdate() {
if (this->_state == STATE::STOP) {
setState(STATE::IDLE);
}
}
void WimaMeasurementArea::init() {
this->setObjectName(WimaMeasurementAreaName);
connect(&this->_tileHeight, &Fact::rawValueChanged, this,
&WimaMeasurementArea::deferUpdate);
connect(&this->_tileWidth, &Fact::rawValueChanged, this,
&WimaMeasurementArea::deferUpdate);
connect(&this->_minTileAreaPercent, &Fact::rawValueChanged, this,
&WimaMeasurementArea::deferUpdate);
connect(this, &WimaArea::pathChanged, this,
&WimaMeasurementArea::deferUpdate);
this->_timer.setSingleShot(true);
connect(&this->_timer, &QTimer::timeout, this,
&WimaMeasurementArea::doUpdate);
connect(&this->_watcher,
&QFutureWatcher<std::unique_ptr<QmlObjectListModel>>::finished, this,
&WimaMeasurementArea::storeTiles);
}
void WimaMeasurementArea::setState(WimaMeasurementArea::STATE s) {
if (this->_state != s) {
auto oldState = this->_state;
this->_state = s;
if (s == STATE::IDLE || oldState == STATE::IDLE) {
emit readyChanged();
}
}
}
/*!
* \class WimaMeasurementArea
* \brief Class defining the area inside which the actual drone measurements
* are performed.
*
* \sa WimaArea, WimaController, WimaPlaner
*/
src/Wima/Geometry/WimaMeasurementArea.h.orig deleted 100644 → 0
View file @ 5f0f205b
#pragma once
#include <QFutureWatcher>
#include <QObject>
#include <QTimer>
#include "WimaArea.h"
#include "SettingsFact.h"
class TileData : public QObject {
public:
QmlObjectListModel tiles;
QVariantList tileCenterPoints;
TileData();
~TileData();
TileData &operator=(const TileData &other);
bool operator==(const TileData &other) const;
bool operator!=(const TileData &other) const;
void clear();
std::size_t size() const;
};
class WimaMeasurementArea : public WimaArea {
Q_OBJECT
enum class STATE { IDLE, DEFERED, UPDATEING, RESTARTING, STOP };
public:
WimaMeasurementArea(QObject *parent = nullptr);
WimaMeasurementArea(const WimaMeasurementArea &other,
QObject *parent = nullptr);
WimaMeasurementArea &operator=(const WimaMeasurementArea &other);
~WimaMeasurementArea();
Q_PROPERTY(Fact *tileHeight READ tileHeight CONSTANT)
Q_PROPERTY(Fact *tileWidth READ tileWidth CONSTANT)
Q_PROPERTY(Fact *minTileArea READ minTileArea CONSTANT)
Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT)
Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged)
Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged)
Q_PROPERTY(QVector<int> progress READ progressQml NOTIFY progressChanged)
// Overrides from WimaPolygon
QString mapVisualQML(void) const;
QString editorQML(void) const;
// Property getters.
Fact *tileHeight();
Fact *tileWidth();
Fact *minTileArea();
Fact *showTiles();
QmlObjectListModel *tiles();
const QVector<int> &progress() const;
QVector<int> progressQml() const;
const QmlObjectListModel *tiles() const;
const QVariantList &tileCenterPoints() const; // List of QGeoCoordinate
const TileData &tileData() const;
int maxTiles() const;
bool ready() const;
// Member Methodes
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// Static Variables
static const char *settingsGroup;
static const char *tileHeightName;
static const char *tileWidthName;
static const char *minTileAreaName;
static const char *showTilesName;
static const char *WimaMeasurementAreaName;
signals:
void tilesChanged();
void maxTilesChanged();
void progressChanged();
void progressAccepted();
void progressNotAccepted();
void readyChanged();
public slots:
bool setProgress(const QVector<int> &p);
private slots:
void doUpdate();
void deferUpdate();
void storeTiles();
void disableUpdate();
void enableUpdate();
private:
// Member Methodes
void init();
void setState(STATE s);
// Members
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _tileHeight;
SettingsFact _tileWidth;
<<<<<<< HEAD
SettingsFact _minTileAreaPercent; // 0..100
=======
SettingsFact _minTileArea;
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
SettingsFact _showTiles;
QVector<int> _progress;
// Tile stuff.
mutable QTimer _timer;
using DataPtr = std::shared_ptr<TileData>;
<<<<<<< HEAD
mutable STATE _state;
mutable TileData _tileData;
mutable QFutureWatcher<DataPtr> _watcher;
=======
mutable TileData _tileData;
mutable QFutureWatcher<DataPtr> _watcher;
mutable STATE _state;
QVector<int> _progress;
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
};
src/Wima/Snake/NemoInterface.cpp.orig deleted 100644 → 0
View file @ 5f0f205b
#include "NemoInterface.h"
#include "SnakeTilesLocal.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <shared_mutex>
#include <QTimer>
#include "QNemoHeartbeat.h"
#include "QNemoProgress.h"
#include "Wima/Geometry/WimaMeasurementArea.h"
#include "Wima/Snake/SnakeTile.h"
#include "Wima/Snake/snake.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define EVENT_TIMER_INTERVAL 100 // ms
auto static timeoutInterval = std::chrono::milliseconds(3000);
using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
using UniqueLock = std::unique_lock<std::shared_timed_mutex>;
using SharedLock = std::shared_lock<std::shared_timed_mutex>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
class NemoInterface::Impl {
using TimePoint = std::chrono::time_point<std::chrono::high_resolution_clock>;
public:
Impl(NemoInterface *p);
void start();
void stop();
void setTileData(const TileData &tileData);
bool hasTileData(const TileData &tileData) const;
void setAutoPublish(bool ap);
void setHoldProgress(bool hp);
void publishTileData();
NemoInterface::STATUS status();
QVector<int> progress();
bool running();
private:
void doTopicServiceSetup();
void loop();
static STATUS heartbeatToStatus(
const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb);
//!
//! \brief Publishes tilesENU
//! \pre this->tilesENUMutex must be locked
//!
void publishTilesENU();
//!
//! \brief Publishes ENUOrigin
//! \pre this->ENUOriginMutex must be locked
//!
void publishENUOrigin();
bool setStatus(NemoInterface::STATUS s);
// Data.
SnakeTilesLocal tilesENU;
mutable std::shared_timed_mutex tilesENUMutex;
QGeoCoordinate ENUOrigin;
mutable std::shared_timed_mutex ENUOriginMutex;
QNemoProgress qProgress;
mutable std::shared_timed_mutex progressMutex;
TimePoint nextTimeout;
mutable std::shared_timed_mutex timeoutMutex;
std::atomic<NemoInterface::STATUS> status_;
// Not protected data.
TileData tileData;
// Internals
std::atomic_bool running_;
std::atomic_bool topicServiceSetupDone;
ROSBridgePtr pRosBridge;
QTimer loopTimer;
NemoInterface *parent;
};
using StatusMap = std::map<NemoInterface::STATUS, QString>;
StatusMap statusMap{
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::HEARTBEAT_DETECTED, "Heartbeat Detected"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::TIMEOUT, "Timeout"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"),
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
NemoInterface::Impl::Impl(NemoInterface *p)
: nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
running_(false), topicServiceSetupDone(false), parent(p) {
// ROS Bridge.
WimaSettings *wimaSettings =
qgcApp()->toolbox()->settingsManager()->wimaSettings();
auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
auto setConnectionString = [connectionStringFact, this] {
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
}
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
setConnectionString();
// Periodic.
connect(&this->loopTimer, &QTimer::timeout, [this] { this->loop(); });
this->loopTimer.start(EVENT_TIMER_INTERVAL);
}
void NemoInterface::Impl::start() {
this->running_ = true;
emit this->parent->runningChanged();
}
void NemoInterface::Impl::stop() {
this->running_ = false;
emit this->parent->runningChanged();
}
void NemoInterface::Impl::setTileData(const TileData &tileData) {
this->tileData = tileData;
if (tileData.tiles.count() > 0) {
std::lock(this->ENUOriginMutex, this->tilesENUMutex);
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
const auto *obj = tileData.tiles.get(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 {
<<<<<<< HEAD
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
=======
qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
break;
}
}
} else {
<<<<<<< HEAD
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
}
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
=======
qWarning() << "NemoInterface::Impl::setTileData(): Origin invalid.";
}
} else {
qWarning() << "NemoInterface::Impl::setTileData(): tile empty.";
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
}
}
} else {
this->tileData.clear();
std::lock(this->ENUOriginMutex, this->tilesENUMutex);
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
this->ENUOrigin = QGeoCoordinate(0, 0, 0);
this->tilesENU = SnakeTilesLocal();
}
}
bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
return this->tileData == tileData;
}
void NemoInterface::Impl::publishTileData() {
std::lock(this->ENUOriginMutex, this->tilesENUMutex);
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
if (this->tilesENU.polygons().size() > 0 && this->running_ &&
this->topicServiceSetupDone) {
this->publishENUOrigin();
this->publishTilesENU();
}
}
NemoInterface::STATUS NemoInterface::Impl::status() { return status_.load(); }
QVector<int> NemoInterface::Impl::progress() {
SharedLock lk(this->progressMutex);
return this->qProgress.progress();
}
bool NemoInterface::Impl::running() { return this->running_.load(); }
void NemoInterface::Impl::doTopicServiceSetup() {
using namespace ros_bridge::messages;
// snake tiles.
{
SharedLock lk(this->tilesENUMutex);
this->pRosBridge->advertiseTopic(
"/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
}
// snake origin.
{
SharedLock lk(this->ENUOriginMutex);
this->pRosBridge->advertiseTopic(
"/snake/origin", geographic_msgs::geo_point::messageType().c_str());
}
// Subscribe nemo progress.
this->pRosBridge->subscribe(
"/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc) {
std::lock(this->progressMutex, this->tilesENUMutex,
this->ENUOriginMutex);
UniqueLock lk1(this->progressMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);
int requiredSize = this->tilesENU.polygons().size();
auto &progressMsg = this->qProgress;
if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
progressMsg.progress().size() !=
requiredSize) { // Some error occured.
progressMsg.progress().clear();
qgcApp()->showMessage("Invalid progress message received.");
}
emit this->parent->progressChanged();
lk1.unlock();
lk2.unlock();
lk3.unlock();
});
// Subscribe /nemo/heartbeat.
this->pRosBridge->subscribe(
"/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc) {
// auto start = std::chrono::high_resolution_clock::now();
nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
this->setStatus(STATUS::INVALID_HEARTBEAT);
} else {
this->setStatus(heartbeatToStatus(heartbeatMsg));
}
if (this->status_ == STATUS::INVALID_HEARTBEAT) {
UniqueLock lk(this->timeoutMutex);
this->nextTimeout = TimePoint::max();
} else if (this->status_ == STATUS::HEARTBEAT_DETECTED) {
UniqueLock lk(this->timeoutMutex);
this->nextTimeout =
std::chrono::high_resolution_clock::now() + timeoutInterval;
}
// auto delta =
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() - start);
// std::cout << "/nemo/heartbeat callback time: " <<
// delta.count() << " ms"
// << std::endl;
});
// Advertise /snake/get_origin.
this->pRosBridge->advertiseService(
"/snake/get_origin", "snake_msgs/GetOrigin",
[this](JsonDocUPtr) -> JsonDocUPtr {
using namespace ros_bridge::messages;
SharedLock lk(this->ENUOriginMutex);
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
auto &origin = this->ENUOrigin;
rapidjson::Value jOrigin(rapidjson::kObjectType);
lk.unlock();
if (geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
<< "/snake/get_origin service: could not create json document.";
}
return pDoc;
});
// Advertise /snake/get_tiles.
this->pRosBridge->advertiseService(
"/snake/get_tiles", "snake_msgs/GetTiles",
[this](JsonDocUPtr) -> JsonDocUPtr {
SharedLock lk(this->tilesENUMutex);
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
<< "/snake/get_tiles service: could not create json document.";
}
return pDoc;
});
}
void NemoInterface::Impl::loop() {
// Check ROS Bridge status and do setup if necessary.
if (this->running_) {
if (!this->pRosBridge->isRunning()) {
this->pRosBridge->start();
this->loop();
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
this->doTopicServiceSetup();
this->topicServiceSetupDone = true;
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else if (this->pRosBridge->isRunning() &&
!this->pRosBridge->connected() && this->topicServiceSetupDone) {
this->pRosBridge->reset();
this->pRosBridge->start();
this->topicServiceSetupDone = false;
this->setStatus(STATUS::TIMEOUT);
}
} else if (this->pRosBridge->isRunning()) {
this->pRosBridge->reset();
this->topicServiceSetupDone = false;
}
// Check if heartbeat timeout occured.
if (this->running_ && this->topicServiceSetupDone) {
UniqueLock lk(this->timeoutMutex);
if (this->nextTimeout != TimePoint::max() &&
this->nextTimeout < std::chrono::high_resolution_clock::now()) {
lk.unlock();
if (this->pRosBridge->isRunning() && this->pRosBridge->connected()) {
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else {
this->setStatus(STATUS::TIMEOUT);
}
}
}
}
NemoInterface::STATUS NemoInterface::Impl::heartbeatToStatus(
const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb) {
if (STATUS(hb.status()) == STATUS::HEARTBEAT_DETECTED)
return STATUS::HEARTBEAT_DETECTED;
else
return STATUS::INVALID_HEARTBEAT;
}
void NemoInterface::Impl::publishTilesENU() {
using namespace ros_bridge::messages;
JsonDocUPtr jSnakeTiles(
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");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishTilesENU: could not create json document.";
}
}
void NemoInterface::Impl::publishENUOrigin() {
using namespace ros_bridge::messages;
JsonDocUPtr jOrigin(
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");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishENUOrigin: could not create json document.";
}
}
bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) {
if (s != this->status_) {
this->status_ = s;
emit this->parent->statusChanged();
return true;
} else {
return false;
}
}
bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) {
if (s != this->status_) {
this->status_ = s;
emit this->parent->statusChanged();
return true;
} else {
return false;
}
}
// ===============================================================
// NemoInterface
NemoInterface::NemoInterface(QObject *parent)
: QObject(parent), pImpl(std::make_unique<NemoInterface::Impl>(this)) {}
NemoInterface::~NemoInterface() {}
void NemoInterface::start() { this->pImpl->start(); }
void NemoInterface::stop() { this->pImpl->stop(); }
void NemoInterface::publishTileData() { this->pImpl->publishTileData(); }
void NemoInterface::requestProgress() {
qCWarning(NemoInterfaceLog) << "requestProgress(): dummy.";
}
void NemoInterface::setTileData(const TileData &tileData) {
this->pImpl->setTileData(tileData);
}
bool NemoInterface::hasTileData(const TileData &tileData) const {
return this->pImpl->hasTileData(tileData);
}
int NemoInterface::status() const { return integral(this->pImpl->status()); }
NemoInterface::STATUS NemoInterface::statusEnum() const {
return this->pImpl->status();
}
QString NemoInterface::statusString() const {
return statusMap.at(this->pImpl->status());
}
QVector<int> NemoInterface::progress() const { return this->pImpl->progress(); }
QString NemoInterface::editorQml() {
return QStringLiteral("NemoInterface.qml");
}
bool NemoInterface::running() { return this->pImpl->running(); }
src/Wima/WimaPlanData_old.cc deleted 100644 → 0
View file @ 5f0f205b
#include "WimaPlanData.h"
WimaPlanData::WimaPlanData(QObject *parent) : QObject(parent) {}
WimaPlanData::WimaPlanData(const WimaPlanData &other, QObject *parent)
: QObject(parent) {
*this = other;
}
/*!
* \fn WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other)
*
* Copies the data area list of \a other to the calling \c WimaPlanData object.
* Returns a reference to the calling \c WimaPlanData object.
*/
WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other) {
// copy wima areas
QList<const WimaAreaData *> areaList = other.areaList();
_areaList.clear();
for (int i = 0; i < areaList.size(); i++) {
const WimaAreaData *areaData = areaList[i];
// determine area type and append
if (areaData->type() == WimaJoinedAreaData::typeString) {
this->append(*qobject_cast<const WimaJoinedAreaData *>(areaData));
} else if (areaData->type() == WimaServiceAreaData::typeString) {
this->append(*qobject_cast<const WimaServiceAreaData *>(areaData));
} else if (areaData->type() == WimaMeasurementAreaData::typeString) {
this->append(*qobject_cast<const WimaMeasurementAreaData *>(areaData));
} else if (areaData->type() == WimaCorridorData::typeString) {
this->append(*qobject_cast<const WimaCorridorData *>(areaData));
}
}
// copy mission items
_missionItems = other.missionItems();
return *this;
}
/*!
* \fn void WimaPlanData::append(const WimaAreaData &areaData)
*
* Adds a WimaAreaData item.
*/
void WimaPlanData::append(const WimaJoinedAreaData &areaData) {
_joinedArea = areaData;
if (!_areaList.contains(&_joinedArea)) {
_areaList.append(&_joinedArea);
}
}
/*!
* \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
*
* Adds a WimaServiceAreaData item.
*/
void WimaPlanData::append(const WimaServiceAreaData &areaData) {
_serviceArea = areaData;
if (!_areaList.contains(&_serviceArea)) {
_areaList.append(&_serviceArea);
}
}
/*!
* \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
*
* Adds a WimaCorridorData item.
*/
void WimaPlanData::append(const WimaCorridorData &areaData) {
_corridor = areaData;
if (!_areaList.contains(&_corridor)) {
_areaList.append(&_corridor);
}
}
/*!
* \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
*
* Adds a WimaMeasurementAreaData item.
*/
void WimaPlanData::append(const WimaMeasurementAreaData &areaData) {
_measurementArea = areaData;
if (!_areaList.contains(&_measurementArea)) {
_areaList.append(&_measurementArea);
}
}
void WimaPlanData::append(const QList<MissionItem *> &missionItems) {
for (auto *item : missionItems) {
item->setParent(this);
_missionItems.append(item);
}
}
/*!
* \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
*
* Clears all stored objects
*/
void WimaPlanData::clear() {
_areaList.clear();
_missionItems.clear();
}
const QList<const WimaAreaData *> &WimaPlanData::areaList() const {
return _areaList;
}
const QList<MissionItem> &WimaPlanData::missionItems() const {
return _missionItems;
}
/*!
* \class WimaPlanData
* \brief Class storing data generated by the \c WimaPlaner class.
*
* This class is designed to store data generated by the \c WimaPlaner class and
* meant for data exchange between the \c WimaController and the \c WimaPlanner.
* It stores a QList of \c WimaAreaData objects, called area data list,
* containing the data of serveral \c WimaAreas generated by the \c WimaPlaner.
*
* \sa QList
*/
src/Wima/WimaPlaner.cc.orig deleted 100644 → 0
View file @ 5f0f205b
#include "WimaPlaner.h"
#include "MissionController.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
#include "QGCMapPolygon.h"
#include "SimpleMissionItem.h"
#include "Geometry/GeoUtilities.h"
#include "Geometry/PlanimetryCalculus.h"
#include "OptimisationTools.h"
#include "CircularSurvey.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaAreaData.h"
#include "WimaBridge.h"
#include "StateMachine.h"
using namespace wima_planer_detail;
#include <functional>
QGC_LOGGING_CATEGORY(WimaPlanerLog, "WimaPlanerLog")
class CommandRAII {
std::function<void(void)> f;
public:
CommandRAII(const std::function<void(void)> &fun) : f(fun) {}
~CommandRAII() { f(); }
};
const char *WimaPlaner::wimaFileExtension = "wima";
const char *WimaPlaner::areaItemsName = "AreaItems";
const char *WimaPlaner::missionItemsName = "MissionItems";
WimaPlaner::WimaPlaner(QObject *parent)
: QObject(parent), _masterController(nullptr), _missionController(nullptr),
_currentAreaIndex(-1), _copyMAreaToSurvey(true), _copySAreaToSurvey(true),
_corridorChanged(true), _joinedArea(this), _arrivalPathLength(0),
_returnPathLength(0), _survey(nullptr), _surveyChanged(true),
_synchronized(false), _nemoInterface(this),
_stateMachine(new StateMachine), _areasMonitored(false),
_missionControllerMonitored(false), _progressLocked(false) {
connect(this, &WimaPlaner::currentPolygonIndexChanged, this,
&WimaPlaner::updatePolygonInteractivity);
// Monitoring.
enableAreaMonitoring();
// Mission controller not set at this point. Not enabling monitoring.
#ifndef NDEBUG
// for debugging and testing purpose, remove if not needed anymore
connect(&_autoLoadTimer, &QTimer::timeout, this,
&WimaPlaner::autoLoadMission);
_autoLoadTimer.setSingleShot(true);
_autoLoadTimer.start(300);
#endif
// NemoInterface
connect(&this->_nemoInterface, &NemoInterface::progressChanged, this,
&WimaPlaner::nemoInterfaceProgressChangedHandler);
// StateMachine
connect(this->_stateMachine.get(), &StateMachine::upToDateChanged, this,
&WimaPlaner::needsUpdateChanged);
connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this,
&WimaPlaner::readyForSynchronizationChanged);
connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this,
&WimaPlaner::surveyReadyChanged);
}
WimaPlaner::~WimaPlaner() {}
PlanMasterController *WimaPlaner::masterController() {
return _masterController;
}
MissionController *WimaPlaner::missionController() {
return _missionController;
}
QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; }
int WimaPlaner::currentPolygonIndex() const { return _currentAreaIndex; }
QString WimaPlaner::currentFile() const { return _currentFile; }
QStringList WimaPlaner::loadNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension)
<< tr("All Files (*.*)");
return filters;
}
QStringList WimaPlaner::saveNameFilters() const {
QStringList filters;
filters << tr("Supported types (*.%1 *.%2)")
.arg(wimaFileExtension)
.arg(AppSettings::planFileExtension);
return filters;
}
QString WimaPlaner::fileExtension() const { return wimaFileExtension; }
QGeoCoordinate WimaPlaner::joinedAreaCenter() const {
return _joinedArea.center();
}
NemoInterface *WimaPlaner::nemoInterface() { return &_nemoInterface; }
void WimaPlaner::setMasterController(PlanMasterController *masterC) {
if (_masterController != masterC) {
_masterController = masterC;
emit masterControllerChanged();
}
}
void WimaPlaner::setMissionController(MissionController *missionC) {
if (_missionController != missionC) {
disableMissionControllerMonitoring();
_missionController = missionC;
enableMissionControllerMonitoring();
emit missionControllerChanged();
}
}
void WimaPlaner::setCurrentPolygonIndex(int index) {
if (index >= 0 && index < _visualItems.count() &&
index != _currentAreaIndex) {
_currentAreaIndex = index;
emit currentPolygonIndexChanged(index);
}
}
void WimaPlaner::setProgressLocked(bool l) {
if (this->_progressLocked != l) {
this->_progressLocked = l;
emit progressLockedChanged();
if (!this->_progressLocked) {
if (this->_measurementArea.setProgress(this->_nemoInterface.progress()))
this->_update();
}
}
}
bool WimaPlaner::synchronized() { return _synchronized; }
bool WimaPlaner::needsUpdate() { return !this->_stateMachine->upToDate(); }
bool WimaPlaner::readyForSynchronization() {
return this->_stateMachine->surveyReady();
}
bool WimaPlaner::surveyReady() { return this->_stateMachine->surveyReady(); }
<<<<<<< HEAD
bool WimaPlaner::progressLocked() { return this->_progressLocked; }
=======
>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95
void WimaPlaner::removeArea(int index) {
if (index >= 0 && index < _visualItems.count()) {
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.removeAt(index));
if (area == nullptr) {
qCWarning(WimaPlanerLog)
<< "removeArea(): nullptr catched, internal error.";
return;
}
area->clear();
area->borderPolygon()->clear();
emit visualItemsChanged();
if (_visualItems.count() == 0) {
// this branch is reached if all items are removed
// to guarentee proper behavior, _currentAreaIndex must be set to a
// invalid value, as on constructor init.
resetAllInteractive();
_currentAreaIndex = -1;
return;
}
if (_currentAreaIndex >= _visualItems.count()) {
setCurrentPolygonIndex(_visualItems.count() - 1);
} else {
updatePolygonInteractivity(_currentAreaIndex);
}
} else {
qCWarning(WimaPlanerLog) << "removeArea(): Index out of bounds!";
}
}
bool WimaPlaner::addMeasurementArea() {
if (!_visualItems.contains(&_measurementArea)) {
_visualItems.append(&_measurementArea);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
} else {
return false;
}
}
bool WimaPlaner::addServiceArea() {
if (!_visualItems.contains(&_serviceArea)) {
_visualItems.append(&_serviceArea);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
} else {
return false;
}
}
bool WimaPlaner::addCorridor() {
if (!_visualItems.contains(&_corridor)) {
_visualItems.append(&_corridor);
int newIndex = _visualItems.count() - 1;
setCurrentPolygonIndex(newIndex);
emit visualItemsChanged();
return true;
} else {
return false;
}
}
void WimaPlaner::removeAll() {
bool changesApplied = false;
// Delete Pointers.
while (_visualItems.count() > 0) {
removeArea(0);
changesApplied = true;
}
// Reset Items.
_measurementArea = WimaMeasurementArea();
_joinedArea = WimaJoinedArea();
_serviceArea = WimaServiceArea();
_corridor = WimaCorridor();
// Remove missions items.
_missionController->removeAll();
_currentFile = "";
_survey = nullptr;
emit currentFileChanged();
if (changesApplied)
emit visualItemsChanged();
}
void WimaPlaner::update() { this->_update(); }
void WimaPlaner::_update() {
setSynchronized(false);
switch (this->_stateMachine->state()) {
case STATE::NEEDS_INIT: {
if (this->_measurementArea.ready()) {
this->_stateMachine->updateState(EVENT::INIT_DONE);
this->_update();
} else {
this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY);
}
} break;
case STATE::WAITING_FOR_TILE_UPDATE: {
} break;
case STATE::NEEDS_J_AREA_UPDATE: {
// check if at least service area and measurement area are available
if (_visualItems.indexOf(&_serviceArea) == -1 ||
_visualItems.indexOf(&_measurementArea) == -1)
return;
// Check if polygons have at least three vertices
if (_serviceArea.count() < 3) {
qgcApp()->showMessage(tr("Service area has less than three vertices."));
return;
}
if (_measurementArea.count() < 3) {
qgcApp()->showMessage(
tr("Measurement area has less than three vertices."));
return;
}
// Check for simple polygons
if (!_serviceArea.isSimplePolygon()) {
qgcApp()->showMessage(tr("Service area is not a simple polygon. "
"Only simple polygons allowed.\n"));
return;
}
if (!_corridor.isSimplePolygon() && _corridor.count() > 0) {
qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only "
"simple polygons allowed.\n"));
return;
}
if (!_measurementArea.isSimplePolygon()) {
qgcApp()->showMessage(tr("Measurement area is not a simple "
"polygon. Only simple polygons allowed.\n"));
return;
}
if (!_serviceArea.containsCoordinate(_serviceArea.depot())) {
qgcApp()->showMessage(tr("Depot not inside service area."));
return;
}
// Join areas.
_joinedArea.setPath(_serviceArea.path());
if (_corridor.count() >= 3) {
_joinedArea.join(_corridor);
}
if (!_joinedArea.join(_measurementArea)) {
qgcApp()->showMessage(
tr("Not able to join areas. Service and measurement area"
" must be overlapping, or connected through a "
"corridor."));
return;
}
this->_stateMachine->updateState(EVENT::J_AREA_UPDATED);
this->_update();
} break; // STATE::NEEDS_J_AREA_UPDATE
case STATE::NEEDS_SURVEY_UPDATE: {
// Need to insert Survey?
QmlObjectListModel *missionItems = _missionController->visualItems();
int surveyIndex = missionItems->indexOf(_survey);
// Create survey item if not yet present.
if (surveyIndex < 0) {
_missionController->insertComplexMissionItem(
_missionController->circularSurveyComplexItemName(),
_measurementArea.center(), missionItems->count());
_survey = qobject_cast<CircularSurvey *>(
missionItems->get(missionItems->count() - 1));
if (_survey == nullptr) {
qCWarning(WimaPlanerLog) << "_survey == nullptr";
return;
}
// establish connections
connect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
connect(_survey, &CircularSurvey::missionItemReady, this,
&WimaPlaner::CSMissionItemReadyHandler);
connect(_survey, &CircularSurvey::destroyed, this,
&WimaPlaner::CSDestroyedHandler);
}
// update survey area
disconnect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
WimaPlanData planData;
if (!toPlanData(planData)) {
qCWarning(WimaPlanerLog) << "not able to create plan data.";
return;
}
_survey->setPlanData(planData);
connect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
// Folloing statement just for completeness.
this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED);
} break; // STATE::NEEDS_SURVEY_UPDATE
case STATE::WAITING_FOR_SURVEY_UPDATE: {
} break;
case STATE::NEEDS_PATH_UPDATE: {
// Check if survey is present.
QmlObjectListModel *missionItems = _missionController->visualItems();
int surveyIndex = missionItems->indexOf(_survey);
if (surveyIndex < 0) {
this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
this->_update();
} else {
// Remove old arrival and return path.
int size = missionItems->count();
for (int i = surveyIndex + 1; i < size; i++)
_missionController->removeMissionItem(surveyIndex + 1);
for (int i = surveyIndex - 1; i > 1; i--)
_missionController->removeMissionItem(i);
// set home position to serArea center
MissionSettingsItem *settingsItem =
qobject_cast<MissionSettingsItem *>(missionItems->get(0));
if (settingsItem == nullptr) {
qCWarning(WimaPlanerLog) << "update(): settingsItem == nullptr";
return;
}
// set altitudes
auto depot = _serviceArea.depot();
depot.setAltitude(0);
settingsItem->setCoordinate(depot);
// set takeoff position
bool setCommandNeeded = false;
if (missionItems->count() < 3) {
setCommandNeeded = true;
_missionController->insertSimpleMissionItem(depot, 1);
}
SimpleMissionItem *takeOffItem =
qobject_cast<SimpleMissionItem *>(missionItems->get(1));
if (takeOffItem == nullptr) {
qCWarning(WimaPlanerLog) << "update(): takeOffItem == nullptr";
return;
}
if (setCommandNeeded)
_missionController->setTakeoffCommand(*takeOffItem);
takeOffItem->setCoordinate(depot);
if (_survey->visualTransectPoints().size() == 0) {
qCWarning(WimaPlanerLog) << "update(): survey no points";
return;
}
// calculate path from take off to survey
QGeoCoordinate start = depot;
QGeoCoordinate end = _survey->coordinate();
QVector<QGeoCoordinate> path;
if (!shortestPath(start, end, path)) {
qgcApp()->showMessage(
QString(tr("Not able to calculate path from "
"takeoff position to measurement area."))
.toLocal8Bit()
.data());
return;
}
_arrivalPathLength = path.size() - 1;
for (int i = 1; i < path.count() - 1; i++) {
(void)_missionController->insertSimpleMissionItem(
path[i], missionItems->count() - 1);
}
// calculate return path
start = _survey->exitCoordinate();
end = depot;
path.clear();
if (!shortestPath(start, end, path)) {
qgcApp()->showMessage(
QString(tr("Not able to calculate the path from "
"measurement area to landing position."))
.toLocal8Bit()
.data());
return;
}
_returnPathLength =
path.size() - 1; // -1: fist item is last measurement point
for (int i = 1; i < path.count() - 1; i++) {
(void)_missionController->insertSimpleMissionItem(
path[i], missionItems->count());
}
// Add waypoint (rover ignores land command).
(void)_missionController->insertSimpleMissionItem(depot,
missionItems->count());
// create land position item
(void)_missionController->insertSimpleMissionItem(depot,
missionItems->count());
SimpleMissionItem *landItem = qobject_cast<SimpleMissionItem *>(
missionItems->get(missionItems->count() - 1));
if (landItem == nullptr) {
qCWarning(WimaPlanerLog) << "update(): landItem == nullptr";
return;
}
if (!_missionController->setLandCommand(*landItem))
return;
this->_stateMachine->updateState(EVENT::PATH_UPDATED);
}
} break; // STATE::NEEDS_PATH_UPDATE
case STATE::UP_TO_DATE: {
} break; // STATE::UP_TO_DATE
} // switch
}
void WimaPlaner::CSDestroyedHandler() {
this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
}
void WimaPlaner::CSMissionItemReadyHandler() {
this->_stateMachine->updateState(EVENT::SURVEY_UPDATED);
this->_update();
}
void WimaPlaner::CSCalculatingChangedHandler() {
if (this->_survey->calculating()) {
this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED);
}
}
void WimaPlaner::mAreaPathChangedHandler() {
this->_stateMachine->updateState(EVENT::M_AREA_PATH_CHANGED);
}
void WimaPlaner::mAreaTilesChangedHandler() {
this->_nemoInterface.setTileData(this->_measurementArea.tileData());
this->_stateMachine->updateState(EVENT::M_AREA_TILES_CHANGED);
}
void WimaPlaner::mAreaProgressChangedHandler() {
this->_stateMachine->updateState(EVENT::M_AREA_PROGRESS_CHANGED);
}
void WimaPlaner::mAreaProgressAcceptedHandler() { this->_update(); }
void WimaPlaner::mAreaReadyChangedHandler() {
if (this->_measurementArea.ready()) {
this->_stateMachine->updateState(EVENT::M_AREA_READY);
} else {
this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY);
}
}
void WimaPlaner::sAreaPathChangedHandler() {
this->_stateMachine->updateState(EVENT::S_AREA_PATH_CHANGED);
}
void WimaPlaner::corridorPathChangedHandler() {
this->_stateMachine->updateState(EVENT::CORRIDOR_PATH_CHANGED);
}
void WimaPlaner::depotChangedHandler() {
this->_stateMachine->updateState(EVENT::DEPOT_CHANGED);
}
void WimaPlaner::missionControllerVisualItemsChangedHandler() {
// Search for survey.
auto surveyIndex = _missionController->visualItems()->indexOf(_survey);
if (surveyIndex < 0) {
// survey not found.
this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
} else {
this->_stateMachine->updateState(EVENT::PATH_CHANGED);
}
}
void WimaPlaner::missionControllerWaypointPathChangedHandler() {
missionControllerVisualItemsChangedHandler();
}
void WimaPlaner::missionControllerNewItemsFromVehicleHandler() {
this->_stateMachine->updateState(EVENT::MISSION_ITEMS_DESTROYED);
}
void WimaPlaner::missionControllerMissionItemCountChangedHandler() {
missionControllerVisualItemsChangedHandler();
}
void WimaPlaner::nemoInterfaceProgressChangedHandler() {
auto p = this->_nemoInterface.progress();
WimaBridge::instance()->setProgress(p);
if (!progressLocked()) {
this->_measurementArea.setProgress(p);
this->_update();
}
}
void WimaPlaner::saveToCurrent() { saveToFile(_currentFile); }
void WimaPlaner::saveToFile(const QString &filename) {
if (filename.isEmpty()) {
return;
}
QString planFilename = filename;
if (!QFileInfo(filename).fileName().contains(".")) {
planFilename += QString(".%1").arg(wimaFileExtension);
}
QFile file(planFilename);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
qgcApp()->showMessage(
tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
_currentFile.clear();
emit currentFileChanged();
} else {
FileType fileType = FileType::WimaFile;
if (planFilename.contains(QString(".%1").arg(wimaFileExtension))) {
fileType = FileType::WimaFile;
} else if (planFilename.contains(
QString(".%1").arg(AppSettings::planFileExtension))) {
fileType = FileType::PlanFile;
} else {
if (planFilename.contains(".")) {
qgcApp()->showMessage(tr("File format not supported"));
} else {
qgcApp()->showMessage(tr("File without file extension not accepted."));
return;
}
}
QJsonDocument saveDoc = saveToJson(fileType);
file.write(saveDoc.toJson());
if (_currentFile != planFilename) {
_currentFile = planFilename;
emit currentFileChanged();
}
}
}
bool WimaPlaner::loadFromCurrent() { return loadFromFile(_currentFile); }
bool WimaPlaner::loadFromFile(const QString &filename) {
// Remove obsolete connections.
disableAreaMonitoring();
disableMissionControllerMonitoring();
CommandRAII onExit([this] {
this->enableAreaMonitoring();
this->enableMissionControllerMonitoring();
});
// disconnect old survey
if (_survey != nullptr) {
disconnect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
disconnect(_survey, &CircularSurvey::missionItemReady, this,
&WimaPlaner::CSMissionItemReadyHandler);
disconnect(_survey, &CircularSurvey::destroyed, this,
&WimaPlaner::CSDestroyedHandler);
}
setSynchronized(false);
// Precondition.
QString errorString;
QString errorMessage =
tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");
if (filename.isEmpty()) {
return false;
}
QFileInfo fileInfo(filename);
QFile file(filename);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
errorString = file.errorString() + QStringLiteral(" ") + filename;
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
if (fileInfo.suffix() == wimaFileExtension) {
QJsonDocument jsonDoc;
QByteArray bytes = file.readAll();
if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
QJsonObject json = jsonDoc.object();
// AreaItems
QJsonArray areaArray = json[areaItemsName].toArray();
_visualItems.clear();
int validAreaCounter = 0;
for (int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) {
QJsonObject jsonArea = areaArray[i].toObject();
if (jsonArea.contains(WimaArea::areaTypeName) &&
jsonArea[WimaArea::areaTypeName].isString()) {
if (jsonArea[WimaArea::areaTypeName] ==
WimaMeasurementArea::WimaMeasurementAreaName) {
bool success = _measurementArea.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
validAreaCounter++;
_visualItems.append(&_measurementArea);
emit visualItemsChanged();
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaServiceArea::wimaServiceAreaName) {
bool success = _serviceArea.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
validAreaCounter++;
_visualItems.append(&_serviceArea);
emit visualItemsChanged();
} else if (jsonArea[WimaArea::areaTypeName] ==
WimaCorridor::WimaCorridorName) {
bool success = _corridor.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
validAreaCounter++;
_visualItems.append(&_corridor);
emit visualItemsChanged();
} else {
errorString +=
QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName));
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
} else {
errorString += QString(tr("Invalid or non existing entry for %s.\n")
.arg(WimaArea::areaTypeName));
return false;
}
}
_currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(),
fileInfo.completeBaseName().toLocal8Bit().data(),
wimaFileExtension);
emit currentFileChanged();
QJsonObject missionObject = json[missionItemsName].toObject();
QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
// create temporary file with missionItems
QFile temporaryFile;
QString cropedFileName = filename.section("/", 0, -2);
QString temporaryFileName;
for (int i = 0;; i++) {
temporaryFileName =
cropedFileName +
QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
if (!QFile::exists(temporaryFileName)) {
temporaryFile.setFileName(temporaryFileName);
if (temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text)) {
break;
}
}
if (i > 1000) {
qCWarning(WimaPlanerLog)
<< "loadFromFile(): not able to create temporary file.";
return false;
}
}
temporaryFile.write(missionJsonDoc.toJson());
temporaryFile.close();
// load from temporary file
_masterController->loadFromFile(temporaryFileName);
QmlObjectListModel *missionItems = _missionController->visualItems();
_survey = nullptr;
for (int i = 0; i < missionItems->count(); i++) {
_survey = missionItems->value<CircularSurvey *>(i);
if (_survey != nullptr) {
connect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
connect(_survey, &CircularSurvey::missionItemReady, this,
&WimaPlaner::CSMissionItemReadyHandler);
connect(_survey, &CircularSurvey::destroyed, this,
&WimaPlaner::CSDestroyedHandler);
break;
}
}
// remove temporary file
if (!temporaryFile.remove()) {
qCWarning(WimaPlanerLog)
<< "WimaPlaner::loadFromFile(): not able to remove "
"temporary file.";
}
return true;
} else {
errorString += QString(tr("File extension not supported.\n"));
qgcApp()->showMessage(errorMessage.arg(errorString));
return false;
}
}
void WimaPlaner::updatePolygonInteractivity(int index) {
if (index >= 0 && index < _visualItems.count()) {
resetAllInteractive();
WimaArea *interactivePoly =
qobject_cast<WimaArea *>(_visualItems.get(index));
if (interactivePoly != nullptr)
interactivePoly->setWimaAreaInteractive(true);
}
}
void WimaPlaner::synchronize() {
if (readyForSynchronization()) {
WimaPlanData planData;
if (toPlanData(planData)) {
WimaBridge::instance()->setPlanData(planData);
setSynchronized(true);
} else {
qCWarning(WimaPlanerLog) << "error creating plan data.";
}
}
}
bool WimaPlaner::shortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path) {
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0, 0);
QPointF end2D;
QVector<QPointF> path2D;
toCartesian(destination, start, end2D);
bool retVal =
PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
void WimaPlaner::setSynchronized(bool s) {
if (this->_synchronized != s) {
this->_synchronized = s;
emit this->synchronizedChanged();
}
}
void WimaPlaner::enableAreaMonitoring() {
if (!areasMonitored()) {
connect(&this->_measurementArea, &WimaArea::pathChanged, this,
&WimaPlaner::mAreaPathChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this,
&WimaPlaner::mAreaTilesChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::progressChanged,
this, &WimaPlaner::mAreaProgressChangedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted,
this, &WimaPlaner::mAreaProgressAcceptedHandler);
connect(&this->_measurementArea, &WimaMeasurementArea::readyChanged, this,
&WimaPlaner::mAreaReadyChangedHandler);
connect(&this->_serviceArea, &WimaArea::pathChanged, this,
&WimaPlaner::sAreaPathChangedHandler);
connect(&this->_serviceArea, &WimaServiceArea::depotChanged, this,
&WimaPlaner::depotChangedHandler);
connect(&this->_corridor, &WimaArea::pathChanged, this,
&WimaPlaner::corridorPathChangedHandler);
this->_areasMonitored = true;
}
}
void WimaPlaner::disableAreaMonitoring() {
if (areasMonitored()) {
disconnect(&this->_measurementArea, &WimaArea::pathChanged, this,
&WimaPlaner::mAreaPathChangedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged,
this, &WimaPlaner::mAreaTilesChangedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::progressChanged,
this, &WimaPlaner::mAreaProgressChangedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted,
this, &WimaPlaner::mAreaProgressAcceptedHandler);
disconnect(&this->_measurementArea, &WimaMeasurementArea::readyChanged,
this, &WimaPlaner::mAreaReadyChangedHandler);
disconnect(&this->_serviceArea, &WimaArea::pathChanged, this,
&WimaPlaner::sAreaPathChangedHandler);
disconnect(&this->_serviceArea, &WimaServiceArea::depotChanged, this,
&WimaPlaner::depotChangedHandler);
disconnect(&this->_corridor, &WimaArea::pathChanged, this,
&WimaPlaner::corridorPathChangedHandler);
this->_areasMonitored = false;
}
}
void WimaPlaner::enableMissionControllerMonitoring() {
if (!missionControllerMonitored() && this->missionController() != nullptr) {
connect(this->missionController(), &MissionController::visualItemsChanged,
this, &WimaPlaner::missionControllerVisualItemsChangedHandler);
connect(this->missionController(), &MissionController::waypointPathChanged,
this, &WimaPlaner::missionControllerWaypointPathChangedHandler);
connect(this->missionController(), &MissionController::newItemsFromVehicle,
this, &WimaPlaner::missionControllerNewItemsFromVehicleHandler);
connect(this->missionController(),
&MissionController::missionItemCountChanged, this,
&WimaPlaner::missionControllerMissionItemCountChangedHandler);
this->_missionControllerMonitored = true;
}
}
void WimaPlaner::disableMissionControllerMonitoring() {
if (missionControllerMonitored() && this->missionController() != nullptr) {
disconnect(this->missionController(),
&MissionController::visualItemsChanged, this,
&WimaPlaner::missionControllerVisualItemsChangedHandler);
disconnect(this->missionController(),
&MissionController::waypointPathChanged, this,
&WimaPlaner::missionControllerWaypointPathChangedHandler);
disconnect(this->missionController(),
&MissionController::newItemsFromVehicle, this,
&WimaPlaner::missionControllerNewItemsFromVehicleHandler);
disconnect(this->missionController(),
&MissionController::missionItemCountChanged, this,
&WimaPlaner::missionControllerMissionItemCountChangedHandler);
this->_missionControllerMonitored = false;
}
}
bool WimaPlaner::areasMonitored() { return this->_areasMonitored; }
bool WimaPlaner::missionControllerMonitored() {
return this->_missionControllerMonitored;
}
void WimaPlaner::resetAllInteractive() {
// Marks all areas as inactive (area.interactive == false)
int itemCount = _visualItems.count();
if (itemCount > 0) {
for (int i = 0; i < itemCount; i++) {
WimaArea *iteratorPoly = qobject_cast<WimaArea *>(_visualItems.get(i));
iteratorPoly->setWimaAreaInteractive(false);
}
}
}
void WimaPlaner::setInteractive() {
updatePolygonInteractivity(_currentAreaIndex);
}
/*!
* \fn WimaPlanData WimaPlaner::toPlanData()
*
* Returns a \c WimaPlanData object containing information about the current
* mission. The \c WimaPlanData object holds only the data which is relevant
* for the \c WimaController class. Should only be called if update() was
* successful.
*
* \sa WimaController, WimaPlanData
*/
bool WimaPlaner::toPlanData(WimaPlanData &planData) {
// store areas
planData.set(WimaMeasurementAreaData(_measurementArea));
planData.set(WimaServiceAreaData(_serviceArea));
planData.set(WimaCorridorData(_corridor));
planData.set(WimaJoinedAreaData(_joinedArea));
return true;
}
#ifndef NDEBUG
void WimaPlaner::autoLoadMission() {
loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/"
"KlingenbachTest.wima");
synchronize();
}
#endif
QJsonDocument WimaPlaner::saveToJson(FileType fileType) {
/// This function save all areas (of WimaPlaner) and all mission items (of
/// MissionController) to a QJsonDocument
/// @param fileType is either WimaFile or PlanFile (enum), if fileType ==
/// PlanFile only mission items are stored
QJsonObject json;
if (fileType == FileType::WimaFile) {
QJsonArray jsonArray;
for (int i = 0; i < _visualItems.count(); i++) {
QJsonObject json;
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.get(i));
if (area == nullptr) {
qCWarning(WimaPlanerLog) << "saveing, area == nullptr!";
return QJsonDocument();
}
// check the type of area, create and append the JsonObject to the
// JsonArray once determined
WimaMeasurementArea *opArea = qobject_cast<WimaMeasurementArea *>(area);
if (opArea != nullptr) {
opArea->saveToJson(json);
jsonArray.append(json);
continue;
}
WimaServiceArea *serArea = qobject_cast<WimaServiceArea *>(area);
if (serArea != nullptr) {
serArea->saveToJson(json);
jsonArray.append(json);
continue;
}
WimaCorridor *corridor = qobject_cast<WimaCorridor *>(area);
if (corridor != nullptr) {
corridor->saveToJson(json);
jsonArray.append(json);
continue;
}
// if non of the obove branches was trigger, type must be WimaArea
area->saveToJson(json);
jsonArray.append(json);
}
json[areaItemsName] = jsonArray;
json[missionItemsName] = _masterController->saveToJson().object();
return QJsonDocument(json);
} else if (fileType == FileType::PlanFile) {
return _masterController->saveToJson();
}
return QJsonDocument(json);
}
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