Skip to content
GitLab
Commit 53fdc29e authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files

.old files removed

parent 5f0f205b
Expand all Hide whitespace changes
Inline Side-by-side
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
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment