Commit de75bd19 authored by Valentin Platzgummer's avatar Valentin Platzgummer

nemointerface improved

parent c84b8bee
......@@ -29,20 +29,16 @@ DebugBuild {
DESTDIR = $${OUT_PWD}/debug
DEFINES += DEBUG
#DEFINES += SNAKE_SHOW_TIME
#DEFINES += DEBUG_SRTL
#DEFINES += SNAKE_DEBUG
DEFINES += SNAKE_SHOW_TIME
DEFINES += SHOW_CIRCULAR_SURVEY_TIME
DEFINES += DEBUG_CIRCULAR_SURVEY
#DEFINES += ROS_BRIDGE_DEBUG
DEFINES += ROS_BRIDGE_DEBUG
}
else {
DESTDIR = $${OUT_PWD}/release
#DEFINES += ROS_BRIDGE_DEBUG
DEFINES += SHOW_CIRCULAR_SURVEY_TIME
DEFINES += SNAKE_SHOW_TIME
#DEFINES += SNAKE_DEBUG
#DEFINES += DEBUG_SRTL
DEFINES += NDEBUG
}
......
......@@ -70,7 +70,7 @@ WimaArea::WimaArea(const WimaArea &other, QObject *parent)
*/
WimaArea &WimaArea::operator=(const WimaArea &other) {
QGCMapPolygon::operator=(other);
this->_maxAltitude = other.maxAltitude();
this->setMaxAltitude(other._maxAltitude);
this->setPath(other.path());
return *this;
......
......@@ -58,8 +58,6 @@ const char *WimaMeasurementArea::settingsGroup = "MeasurementArea";
const char *WimaMeasurementArea::tileHeightName = "TileHeight";
const char *WimaMeasurementArea::tileWidthName = "TileWidth";
const char *WimaMeasurementArea::minTileAreaName = "MinTileArea";
const char *WimaMeasurementArea::transectDistanceName = "TransectDistance";
const char *WimaMeasurementArea::minTransectLengthName = "MinTransectLength";
const char *WimaMeasurementArea::showTilesName = "ShowTiles";
const char *WimaMeasurementArea::WimaMeasurementAreaName = "Measurement Area";
......@@ -76,12 +74,6 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)),
_minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName],
this /* QObject parent */)),
_transectDistance(SettingsFact(settingsGroup,
_metaDataMap[transectDistanceName],
this /* QObject parent */)),
_minTransectLength(SettingsFact(settingsGroup,
_metaDataMap[minTransectLengthName],
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -100,12 +92,6 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)),
_minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName],
this /* QObject parent */)),
_transectDistance(SettingsFact(settingsGroup,
_metaDataMap[transectDistanceName],
this /* QObject parent */)),
_minTransectLength(SettingsFact(settingsGroup,
_metaDataMap[minTransectLengthName],
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -120,7 +106,6 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
WimaMeasurementArea &WimaMeasurementArea::
operator=(const WimaMeasurementArea &other) {
WimaArea::operator=(other);
return *this;
}
......@@ -140,10 +125,6 @@ Fact *WimaMeasurementArea::tileWidth() { return &_tileWidth; }
Fact *WimaMeasurementArea::minTileArea() { return &_minTileArea; }
Fact *WimaMeasurementArea::transectDistance() { return &_transectDistance; }
Fact *WimaMeasurementArea::minTransectLength() { return &_minTransectLength; }
Fact *WimaMeasurementArea::showTiles() { return &_showTiles; }
QmlObjectListModel *WimaMeasurementArea::tiles() {
......@@ -179,8 +160,6 @@ void WimaMeasurementArea::saveToJson(QJsonObject &json) {
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileArea.rawValue().toDouble();
json[transectDistanceName] = _transectDistance.rawValue().toDouble();
json[minTransectLengthName] = _minTransectLength.rawValue().toDouble();
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
}
......@@ -211,22 +190,6 @@ bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
retVal = false;
}
if (json.contains(transectDistanceName) &&
json[transectDistanceName].isDouble()) {
_transectDistance.setRawValue(json[transectDistanceName].toDouble());
} else {
errorString.append(tr("Could not load transect distance!\n"));
retVal = false;
}
if (json.contains(minTransectLengthName) &&
json[minTransectLengthName].isDouble()) {
_minTransectLength.setRawValue(json[minTransectLengthName].toDouble());
} else {
errorString.append(tr("Could not load minimal transect length!\n"));
retVal = false;
}
if (json.contains(showTilesName) && json[showTilesName].isBool()) {
_showTiles.setRawValue(json[showTilesName].toBool());
} else {
......
......@@ -44,8 +44,6 @@ public:
Q_PROPERTY(Fact *tileHeight READ tileHeight CONSTANT)
Q_PROPERTY(Fact *tileWidth READ tileWidth CONSTANT)
Q_PROPERTY(Fact *minTileArea READ minTileArea CONSTANT)
Q_PROPERTY(Fact *transectDistance READ transectDistance CONSTANT)
Q_PROPERTY(Fact *minTransectLength READ minTransectLength CONSTANT)
Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT)
Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged)
Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged)
......@@ -55,11 +53,10 @@ public:
QString mapVisualQML(void) const;
QString editorQML(void) const;
// Property getters.
Fact *tileHeight();
Fact *tileWidth();
Fact *minTileArea();
Fact *transectDistance();
Fact *minTransectLength();
Fact *showTiles();
QmlObjectListModel *tiles();
const QVector<int> &progress() const;
......@@ -83,8 +80,6 @@ public:
static const char *tileHeightName;
static const char *tileWidthName;
static const char *minTileAreaName;
static const char *transectDistanceName;
static const char *minTransectLengthName;
static const char *showTilesName;
static const char *WimaMeasurementAreaName;
......@@ -93,6 +88,7 @@ signals:
void maxTilesChanged();
void progressChanged();
void progressAccepted();
void progressNotAccepted();
void readyChanged();
public slots:
......@@ -114,17 +110,14 @@ private:
SettingsFact _tileHeight;
SettingsFact _tileWidth;
SettingsFact _minTileArea;
SettingsFact _transectDistance;
SettingsFact _minTransectLength;
SettingsFact _showTiles;
// Tile stuff.
QTimer _timer;
mutable QTimer _timer;
using DataPtr = std::shared_ptr<TileData>;
TileData _tileData;
QFutureWatcher<DataPtr> _watcher;
STATE _state;
mutable TileData _tileData;
mutable QFutureWatcher<DataPtr> _watcher;
mutable STATE _state;
QVector<int> _progress;
};
......@@ -55,7 +55,7 @@ public:
bool running();
private:
bool doTopicServiceSetup();
void doTopicServiceSetup();
void loop();
static STATUS heartbeatToStatus(
const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb);
......@@ -69,6 +69,7 @@ private:
//! \pre this->ENUOriginMutex must be locked
//!
void publishENUOrigin();
bool setStatus(NemoInterface::STATUS s);
// Data.
SnakeTilesLocal tilesENU;
......@@ -77,9 +78,9 @@ private:
mutable std::shared_timed_mutex ENUOriginMutex;
QNemoProgress qProgress;
mutable std::shared_timed_mutex progressMutex;
NemoInterface::STATUS status_;
TimePoint nextTimeout;
mutable std::shared_timed_mutex statusMutex;
mutable std::shared_timed_mutex timeoutMutex;
std::atomic<NemoInterface::STATUS> status_;
// Not protected data.
TileData tileData;
......@@ -106,7 +107,7 @@ StatusMap statusMap{
NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
NemoInterface::Impl::Impl(NemoInterface *p)
: status_(STATUS::NOT_CONNECTED), nextTimeout(TimePoint::max()),
: nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
running_(false), topicServiceSetupDone(false), parent(p) {
// ROS Bridge.
......@@ -158,25 +159,37 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
if (tile->coordinateList().size() > 0) {
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 {
qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
break;
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 {
qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
break;
}
}
} else {
qWarning() << "NemoInterface::Impl::setTileData(): Origin invalid.";
}
} else {
qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
qWarning() << "NemoInterface::Impl::setTileData(): tile empty.";
}
}
} 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();
}
}
......@@ -196,10 +209,7 @@ void NemoInterface::Impl::publishTileData() {
}
}
NemoInterface::STATUS NemoInterface::Impl::status() {
SharedLock lk(this->statusMutex);
return status_;
}
NemoInterface::STATUS NemoInterface::Impl::status() { return status_.load(); }
QVector<int> NemoInterface::Impl::progress() {
SharedLock lk(this->progressMutex);
......@@ -208,15 +218,12 @@ QVector<int> NemoInterface::Impl::progress() {
bool NemoInterface::Impl::running() { return this->running_.load(); }
bool NemoInterface::Impl::doTopicServiceSetup() {
void NemoInterface::Impl::doTopicServiceSetup() {
using namespace ros_bridge::messages;
// snake tiles.
{
SharedLock lk(this->tilesENUMutex);
if (this->tilesENU.polygons().size() == 0)
return false;
this->pRosBridge->advertiseTopic(
"/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
......@@ -225,7 +232,6 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
// snake origin.
{
SharedLock lk(this->ENUOriginMutex);
this->pRosBridge->advertiseTopic(
"/snake/origin", geographic_msgs::geo_point::messageType().c_str());
}
......@@ -261,21 +267,20 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
/* callback */ [this](JsonDocUPtr pDoc) {
// auto start = std::chrono::high_resolution_clock::now();
nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
UniqueLock lk(this->statusMutex);
if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
status_ = STATUS::INVALID_HEARTBEAT;
this->setStatus(STATUS::INVALID_HEARTBEAT);
} else {
status_ = heartbeatToStatus(heartbeatMsg);
this->setStatus(heartbeatToStatus(heartbeatMsg));
}
if (status_ == STATUS::INVALID_HEARTBEAT) {
if (this->status_ == STATUS::INVALID_HEARTBEAT) {
UniqueLock lk(this->timeoutMutex);
this->nextTimeout = TimePoint::max();
} else if (status_ == STATUS::HEARTBEAT_DETECTED) {
} else if (this->status_ == STATUS::HEARTBEAT_DETECTED) {
UniqueLock lk(this->timeoutMutex);
this->nextTimeout =
std::chrono::high_resolution_clock::now() + timeoutInterval;
}
lk.unlock();
emit this->parent->statusChanged();
// auto delta =
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() - start);
......@@ -320,8 +325,6 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
return pDoc;
});
return true;
}
void NemoInterface::Impl::loop() {
......@@ -329,25 +332,20 @@ void NemoInterface::Impl::loop() {
if (this->running_) {
if (!this->pRosBridge->isRunning()) {
this->pRosBridge->start();
this->loop();
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
if (this->doTopicServiceSetup()) {
this->topicServiceSetupDone = true;
UniqueLock lk(this->statusMutex);
this->status_ = STATUS::WEBSOCKET_DETECTED;
lk.unlock();
emit this->parent->statusChanged();
}
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;
UniqueLock lk(this->statusMutex);
this->status_ = STATUS::TIMEOUT;
lk.unlock();
emit this->parent->statusChanged();
this->setStatus(STATUS::TIMEOUT);
}
} else if (this->pRosBridge->isRunning()) {
this->pRosBridge->reset();
......@@ -356,16 +354,15 @@ void NemoInterface::Impl::loop() {
// Check if heartbeat timeout occured.
if (this->running_ && this->topicServiceSetupDone) {
UniqueLock lk(this->statusMutex);
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->status_ = STATUS::WEBSOCKET_DETECTED;
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else {
this->status_ = STATUS::TIMEOUT;
this->setStatus(STATUS::TIMEOUT);
}
lk.unlock();
emit this->parent->statusChanged();
}
}
}
......@@ -400,6 +397,16 @@ void NemoInterface::Impl::publishENUOrigin() {
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
}
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)
......
......@@ -1143,10 +1143,17 @@ bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
} // namespace snake
bool boost::geometry::model::operator==(snake::FPoint p1, snake::FPoint p2) {
bool boost::geometry::model::operator==(snake::FPoint &p1, snake::FPoint &p2) {
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool boost::geometry::model::operator!=(snake::FPoint p1, snake::FPoint p2) {
bool boost::geometry::model::operator!=(snake::FPoint &p1, snake::FPoint &p2) {
return !(p1 == p2);
}
bool boost::geometry::model::operator==(snake::IPoint &p1, snake::IPoint &p2) {
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool boost::geometry::model::operator!=(snake::IPoint &p1, snake::IPoint &p2) {
return !(p1 == p2);
}
......@@ -258,8 +258,10 @@ namespace boost {
namespace geometry {
namespace model {
bool operator==(snake::FPoint p1, snake::FPoint p2);
bool operator!=(snake::FPoint p1, snake::FPoint p2);
bool operator==(snake::FPoint &p1, snake::FPoint &p2);
bool operator!=(snake::FPoint &p1, snake::FPoint &p2);
bool operator==(snake::IPoint &p1, snake::IPoint &p2);
bool operator!=(snake::IPoint &p1, snake::IPoint &p2);
} // namespace model
} // namespace geometry
......
......@@ -160,8 +160,6 @@ bool WimaPlaner::readyForSynchronization() {
bool WimaPlaner::progressLocked() { return this->_progressLocked; }
WimaPlaner *WimaPlaner::thisPointer() { return this; }
void WimaPlaner::removeArea(int index) {
if (index >= 0 && index < _visualItems.count()) {
WimaArea *area = qobject_cast<WimaArea *>(_visualItems.removeAt(index));
......@@ -239,15 +237,20 @@ bool WimaPlaner::addCorridor() {
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();
......
......@@ -82,7 +82,6 @@ public:
void setCurrentPolygonIndex(int index);
void setProgressLocked(bool l);
Q_INVOKABLE WimaPlaner *thisPointer();
Q_INVOKABLE bool addMeasurementArea();
/// Removes an area from _visualItems
/// @param index Index of the area to be removed
......
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