Skip to content

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

addapting wima stuff to qgc master branch, code not ready

parent 5e177398
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 320 additions and 702 deletions
src/MissionManager/SimpleMissionItem.h
View file @ 99e5ae38
......@@ -118,7 +118,7 @@ public:
QString mapVisualQML (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
ReadyForSaveState readyForSaveState (void) const final;
double additionalTimeDelay (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return true; }
......
src/MissionManager/TransectStyleComplexItem.h
View file @ 99e5ae38
......@@ -88,8 +88,8 @@ public:
// Overrides from VisualMissionItem
void save (QJsonArray& planItems) override = 0;
bool specifiesCoordinate (void) const override = 0;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
virtual void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
virtual void applyNewAltitude (double newAltitude) final;
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
......@@ -100,7 +100,7 @@ public:
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
ReadyForSaveState readyForSaveState (void) const override;
QString commandDescription (void) const override { return tr("Transect"); }
QString commandName (void) const override { return tr("Transect"); }
......
src/Wima/CircularSurvey.cc
View file @ 99e5ae38
......@@ -19,6 +19,8 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")
template <typename T>
......@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::variantName = "Variant";
CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
CircularSurvey::CircularSurvey(PlanMasterController *masterController, bool flyView,
const QString &kmlOrShpFile, QObject *parent)
: TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent),
: TransectStyleComplexItem(masterController, flyView, settingsGroup, parent),
_state(STATE::IDLE),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
......@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&CircularSurvey::_setTransects);
connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this,
&CircularSurvey::calculatingChanged);
this->_transectsDirty = true;
// Altitude
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::coordinateHasRelativeAltitudeChanged);
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::exitCoordinateHasRelativeAltitudeChanged);
// Register Generators.
auto lg = std::make_shared<routing::LinearGenerator>(this->_pAreaData);
......@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return false;
}
if (!_load(complexObject, errorString)) {
if (!load(complexObject, sequenceNumber, errorString)) {
_ignoreRecalc = false;
return false;
}
......@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_ignoreRecalc = false;
_recalcComplexDistance();
if (_cameraShots == 0) {
// Shot count was possibly not available from plan file
_recalcCameraShots();
......@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
bool CircularSurvey::specifiesCoordinate() const { return true; }
void CircularSurvey::appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty)
return;
if (_loadedMissionItems.count()) {
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems(items, missionItemParent);
} else {
// Build the mission items on the fly
_buildAndAppendMissionItems(items, missionItemParent);
}
}
void CircularSurvey::_appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty)
return;
int seqNum = _sequenceNumber;
for (const MissionItem *loadedMissionItem : _loadedMissionItems) {
MissionItem *item = new MissionItem(*loadedMissionItem, missionItemParent);
item->setSequenceNumber(seqNum++);
items.append(item);
}
}
void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty || _transects.count() == 0)
return;
MissionItem *item;
int seqNum = _sequenceNumber;
MAV_FRAME mavFrame =
followTerrain() || !_cameraCalc.distanceToSurfaceRelative()
? MAV_FRAME_GLOBAL
: MAV_FRAME_GLOBAL_RELATIVE_ALT;
for (const QList<TransectStyleComplexItem::CoordInfo_t> &transect :
_transects) {
// bool transectEntry = true;
for (const CoordInfo_t &transectCoordInfo : transect) {
item = new MissionItem(
seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame,
0, // Hold time (delay for hover and capture to settle vehicle
// before image is taken)
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
transectCoordInfo.coord.latitude(),
transectCoordInfo.coord.longitude(),
transectCoordInfo.coord.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
}
}
bool CircularSurvey::_switchToGenerator(
const CircularSurvey::PtrGenerator &newG) {
if (this->_pGenerator != newG) {
......@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
}
void CircularSurvey::_updateWorker() {
// Mark transects as dirty.
this->_transectsDirty = true;
// Reset data.
this->_transects.clear();
this->_variantVector.clear();
......@@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() {
}
}
void CircularSurvey::applyNewAltitude(double newAltitude) {
_cameraCalc.valueSetIsDistance()->setRawValue(true);
_cameraCalc.distanceToSurface()->setRawValue(newAltitude);
_cameraCalc.setDistanceToSurfaceRelative(true);
}
double CircularSurvey::timeBetweenShots() { return 1; }
QString CircularSurvey::commandDescription() const {
......@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString CircularSurvey::abbreviation() const { return tr("C.S."); }
bool CircularSurvey::readyForSave() const {
return TransectStyleComplexItem::readyForSave() && !_transectsDirty &&
this->_state == STATE::IDLE;
TransectStyleComplexItem::ReadyForSaveState
CircularSurvey::readyForSaveState() const {
if (TransectStyleComplexItem::readyForSaveState() ==
TransectStyleComplexItem::ReadyForSaveState::ReadyForSave) {
if (this->_state == STATE::IDLE) {
return ReadyForSaveState::ReadyForSave;
} else {
return ReadyForSaveState::NotReadyForSaveData;
}
} else {
return TransectStyleComplexItem::readyForSaveState();
}
}
double CircularSurvey::additionalTimeDelay() const { return 0; }
......@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
<< " ms";
}
void CircularSurvey::_recalcComplexDistance() {
_complexDistance = 0;
if (_transectsDirty)
return;
for (int i = 0; i < _visualTransectPoints.count() - 1; i++) {
_complexDistance +=
_visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(
_visualTransectPoints[i + 1].value<QGeoCoordinate>());
}
emit complexDistanceChanged();
}
// no cameraShots in Circular Survey, add if desired
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
......@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
&CircularSurvey::_changeVariant);
this->_changeVariantWorker();
// Mark transect ready.
this->_transectsDirty = false;
this->_state = STATE::SKIPP;
this->_rebuildTransects();
} else {
......
src/Wima/CircularSurvey.h
View file @ 99e5ae38
......@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
using PtrGenerator = std::shared_ptr<routing::GeneratorBase>;
using PtrRoutingData = std::shared_ptr<RoutingData>;
using PtrWorker = std::shared_ptr<RoutingThread>;
using PtrWorker = std::unique_ptr<RoutingThread>;
using Transects = QList<QList<CoordInfo_t>>;
using Variant = Transects;
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default
/// polygon
CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile,
CircularSurvey(PlanMasterController *masterController, bool flyView, const QString &kmlOrShpFile,
QObject *parent);
~CircularSurvey();
......@@ -63,14 +60,11 @@ public:
QString mapVisualQML(void) const override final;
void save(QJsonArray &planItems) override final;
bool specifiesCoordinate(void) const override final;
void appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) override final;
void applyNewAltitude(double newAltitude) override final;
double timeBetweenShots(void) override final;
QString commandDescription(void) const override final;
QString commandName(void) const override final;
QString abbreviation(void) const override final;
bool readyForSave(void) const override final;
ReadyForSaveState readyForSaveState(void) const override final;
double additionalTimeDelay(void) const override final;
// Generator
......@@ -97,7 +91,6 @@ signals:
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final;
// Worker functions.
......@@ -108,11 +101,6 @@ private slots:
void _reverseWorker();
private:
void _appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
bool _switchToGenerator(const PtrGenerator &newG);
// State.
......@@ -139,7 +127,6 @@ private:
PtrGenerator _pGenerator;
// Routing.
using Variant = Transects;
QVector<Variant> _variantVector;
PtrWorker _pWorker;
};
src/Wima/Geometry/WimaArea.cc
View file @ 99e5ae38
......@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
} else if (retValue == JoinPolygonError::PathSizeLow) {
qWarning("Polygon vertex count is low.");
} else {
QVector<QGeoCoordinate> path;
QList<QGeoCoordinate> path;
toGeoList(joinedPolygon, origin, path);
// qWarning("after transform");
// qWarning() << path;
......@@ -491,38 +491,6 @@ void WimaArea::init() {
&WimaArea::recalcInteractivity);
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the console.
*/
void print(const WimaArea &area) {
QString message;
print(area, message);
qWarning() << message;
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the \a outputString.
*/
void print(const WimaArea &area, QString &outputString) {
outputString.append(QString("Type: %1\n").arg(area.objectName()));
print(static_cast<const QGCMapPolygon &>(area), outputString);
outputString.append(QString("Maximum Altitude: %1\n").arg(area._maxAltitude));
outputString.append(
QString("Border Polygon Offset: %1\n")
.arg(area._borderPolygonOffset.rawValue().toDouble()));
outputString.append(
QString("Border Polygon Coordinates\n")
.arg(area._borderPolygonOffset.rawValue().toDouble()));
for (int i = 0; i < area._borderPolygon.count(); i++) {
QGeoCoordinate coordinate = area._borderPolygon.vertexCoordinate(i);
outputString.append(
QString("%1\n").arg(coordinate.toString(QGeoCoordinate::Degrees)));
}
}
// QDoc Documentation
/*!
......
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ 99e5ae38
......@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData &TileData::operator=(const TileData &other) {
this->tiles.clearAndDeleteContents();
for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) {
const auto *obj = other.tiles.get(i);
const auto *obj = other.tiles[i];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
......@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
}
bool TileData::operator==(const TileData &other) const {
return this->tiles == other.tiles &&
this->tileCenterPoints == other.tileCenterPoints;
if (this->tileCenterPoints == other.tileCenterPoints &&
this->tiles.count() == other.tiles.count()) {
for (int i = 0; i < other.tiles.count(); ++i) {
if (this->tiles[i] != other.tiles[i]) {
return false;
}
}
return true;
} else {
return false;
}
}
bool TileData::operator!=(const TileData &other) const {
......@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) {
if (ready()) {
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble();
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
} else {
qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing.";
}
......@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
auto start = std::chrono::high_resolution_clock::now();
if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) {
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea;
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea;
// Check some conditions.
if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) {
this->count() >= 3 && this->isSimplePolygon()) {
setState(STATE::UPDATEING);
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto minArea =
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto minArea =
this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea;
auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
}
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
}
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
}
}
pData->moveToThread(th);
pData->moveToThread(th);
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
return pData;
}); // QtConcurrent::run()
return pData;
}); // QtConcurrent::run()
this->_watcher.setFuture(future);
}
this->_watcher.setFuture(future);
}
}
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): execution time: "
......
src/Wima/Geometry/WimaServiceArea.cc
View file @ 99e5ae38
#include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(WimaServiceAreaLog, "WimaServiceAreaLog")
const char *WimaServiceArea::wimaServiceAreaName = "Service Area";
const char *WimaServiceArea::depotLatitudeName = "DepotLatitude";
const char *WimaServiceArea::depotLongitudeName = "DepotLongitude";
......@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
} else if (this->_depot.isValid()) {
// Use nearest coordinate.
auto minDist = std::numeric_limits<double>::infinity();
auto minIt = this->pathReference().begin();
for (auto it = this->pathReference().begin();
it < this->pathReference().end(); ++it) {
const auto vertex = it->value<QGeoCoordinate>();
auto d = vertex.distanceTo(this->_depot);
if (d < minDist) {
minDist = d;
minIt = it;
int minIndex = 0;
for (int idx = 0; idx < this->pathModel().count(); ++idx) {
const QObject *obj = this->pathModel()[idx];
const auto *vertex = qobject_cast<const QGeoCoordinate *>(obj);
if (vertex != nullptr) {
auto d = vertex->distanceTo(this->_depot);
if (d < minDist) {
minDist = d;
minIndex = idx;
}
} else {
qCCritical(WimaServiceAreaLog) << "init(): nullptr catched!";
}
}
this->setDepot(minIt->value<QGeoCoordinate>());
} else if (this->pathReference().size() > 0) {
this->setDepot(*this->pathModel().value<QGeoCoordinate *>(minIndex));
} else if (this->pathModel().count() > 0) {
// Use first coordinate.
this->setDepot(this->pathReference().value(0).value<QGeoCoordinate>());
this->setDepot(*this->pathModel().value<QGeoCoordinate *>(0));
}
}
});
......
src/Wima/Snake/CircularGenerator.cpp
View file @ 99e5ae38
......@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
const auto *tile = tiles->value<const SnakeTile *>(i);
const auto *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
src/Wima/Snake/LinearGenerator.cpp
View file @ 99e5ae38
......@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) {
const auto *tile = tiles->value<const SnakeTile *>(i);
const QObject *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
src/Wima/Snake/NemoInterface.cpp
View file @ 99e5ae38
......@@ -97,7 +97,7 @@ private:
};
using StatusMap = std::map<NemoInterface::STATUS, QString>;
StatusMap statusMap{
static StatusMap statusMap{
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
std::make_pair<NemoInterface::STATUS, QString>(
......@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto connectionString = connectionStringFact->rawValue().toString();
if (ros_bridge::isValidConnectionString(
connectionString.toLocal8Bit().data())) {
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
} else {
QString defaultString("localhost:9090");
qgcApp()->showMessage("ROS Bridge connection string invalid: " +
connectionString);
qgcApp()->showMessage("Resetting connection string to: " + defaultString);
connectionStringFact->setRawValue(
QVariant(defaultString)); // calls this function recursively
qgcApp()->warningMessageBoxOnMainThread(
"Nemo Interface",
"Websocket connection string possibly invalid: " + connectionString +
". Trying to connect anyways.");
}
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
......@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
const auto *obj = tileData.tiles.get(0);
const auto *obj = tileData.tiles[0];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
if (tile->coordinateList().size() > 0) {
if (tile->coordinateList().first().isValid()) {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles.get(i);
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles[i];
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
break;
break;
}
}
}
} else {
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
}
}
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
}
}
}
} else {
this->tileData.clear();
......@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
progressMsg.progress().size() !=
requiredSize) { // Some error occured.
progressMsg.progress().clear();
qgcApp()->showMessage("Invalid progress message received.");
qgcApp()->informationMessageBoxOnMainThread(
"Nemo Interface", "Invalid progress message received.");
}
emit this->parent->progressChanged();
......@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
......@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
......@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
this->doTopicServiceSetup();
this->topicServiceSetupDone = true;
this->topicServiceSetupDone = true;