From cf53d945fed15c794f0b1230336370c2ed6c0138 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 2 Sep 2020 14:01:38 +0200 Subject: [PATCH] SnakeDataManager mod --- qgroundcontrol.pro | 2 +- src/Snake/snake.cpp | 29 ++- src/Snake/snake_typedefs.h | 19 +- src/Wima/Snake/SnakeDataManager.cc | 382 ++++++++++++++++++++++------- src/Wima/Snake/SnakeDataManager.h | 22 +- src/Wima/WimaController.cc | 178 ++------------ 6 files changed, 371 insertions(+), 261 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index edb83f2a5..23f50a5f4 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -441,7 +441,7 @@ HEADERS += \ src/Wima/Snake/SnakeTileLocal.h \ src/Wima/Snake/SnakeTiles.h \ src/Wima/Snake/SnakeTilesLocal.h \ - src/Wima/Snake/SnakeW.h \ + src/Wima/Snake/SnakeDataManager.h \ src/Wima/WaypointManager/AreaInterface.h \ src/Wima/WaypointManager/DefaultManager.h \ src/Wima/WaypointManager/GenericWaypointManager.h \ diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index be83f3061..67c524a62 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -1041,7 +1041,6 @@ bool flight_plan::route(const BoostPolygon &area, route_idx.push_back(manager.IndexToNode(index).value()); } - // Helper Lambda. auto idx2Vertex = [&vertices](const std::vector &idxArray, std::vector &path){ @@ -1054,17 +1053,31 @@ bool flight_plan::route(const BoostPolygon &area, size_t idx0 = route_idx[i]; size_t idx1 = route_idx[i+1]; const auto &info1 = transectInfoList[idx0]; - const auto &info2 = transectInfoList[idx0]; - if ( info.front ){ - const auto &t = transects[info.index]; - for ( auto it = t.begin(); it < t.end()-1; ++it){ - route.push_back(*it); + const auto &info2 = transectInfoList[idx1]; + if (info1.index == info2.index) { // same transect? + if ( !info1.front ) { // transect reversal needed? + BoostLineString reversedTransect; + const auto &t = transects[info1.index]; + for ( auto it = t.end()-1; it >= t.begin(); --it){ + reversedTransect.push_back(*it); + } + transectsRouted.push_back(reversedTransect); + for (auto it = reversedTransect.begin(); it < reversedTransect.end()-1; ++it){ + route.push_back(*it); + } + } else { + const auto &t = transects[info1.index]; + for ( auto it = t.begin(); it < t.end()-1; ++it){ + route.push_back(*it); + } + transectsRouted.push_back(t); } - transectsRouted.push_back(t); } else { std::vector idxList; shortestPathFromGraph(connectionGraph, idx0, idx1, idxList); - idxList.pop_back(); + if ( i != route_idx.size()-2){ + idxList.pop_back(); + } idx2Vertex(idxList, route); } } diff --git a/src/Snake/snake_typedefs.h b/src/Snake/snake_typedefs.h index 141adca23..13abb1b10 100644 --- a/src/Snake/snake_typedefs.h +++ b/src/Snake/snake_typedefs.h @@ -22,8 +22,21 @@ typedef bg::model::linestring BoostLineString; typedef std::vector> Int64Matrix; typedef bg::model::box BoostBox; -typedef bu::quantity Length; -typedef bu::quantity Area; -typedef bu::quantity Angle; +typedef bu::quantity Length; +typedef bu::quantity Area; +typedef bu::quantity Angle; } + +namespace boost{ +namespace geometry{ +namespace model { +bool operator==(snake::BoostPoint p1, snake::BoostPoint p2){ + return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>()); +} +bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2){ + return !(p1 == p2); +} +} // namespace model +} // namespace geometry +} // namespace boost diff --git a/src/Wima/Snake/SnakeDataManager.cc b/src/Wima/Snake/SnakeDataManager.cc index f92b7b886..ab0d1c924 100644 --- a/src/Wima/Snake/SnakeDataManager.cc +++ b/src/Wima/Snake/SnakeDataManager.cc @@ -17,71 +17,74 @@ #include "Wima/Snake/SnakeTiles.h" #include "Wima/Snake/SnakeTilesLocal.h" -#include "comm/ros_bridge/include/ros_bridge.h" +#include "ros_bridge/include/ros_bridge.h" +#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include "ros_bridge/rapidjson/include/rapidjson/writer.h" +#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.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/progress.h" +#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h" + + +#define EVENT_TIMER_INTERVAL 500 // ms +#define TIMEOUT 3000 // ms + using QVariantList = QList; using ROSBridgePtr = std::unique_ptr; using UniqueLock = std::unique_lock; using SharedLock = std::shared_lock; +using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; class SnakeImpl : public QObject{ Q_OBJECT public: - SnakeImpl() : - lineDistance(1*si::meter) - , minTransectLength(1*si::meter) - , calcInProgress(false) - , topicServiceSetupDone(false) - { - // 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 { - qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString); - this->pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090")); - } - }; - setConnectionString(); - connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString); - } + SnakeImpl(SnakeDataManager* p); + + bool precondition() const; + void resetWaypointData(); + bool doTopicServiceSetup(); // Private data. - ROSBridgePtr pRosBridge; - bool topicServiceSetupDone; - mutable std::shared_timed_mutex mutex; - - // Input - snake::Scenario scenario; - Length lineDistance; - Length minTransectLength; + ROSBridgePtr pRosBridge; + bool rosBridgeEnabeled; + bool topicServiceSetupDone; + QTimer eventTimer; + QTimer timeout; + mutable std::shared_timed_mutex mutex; + + // Scenario + snake::Scenario scenario; + Length lineDistance; + Length minTransectLength; QList mArea; QList sArea; QList corridor; + QGeoCoordinate ENUOrigin; + SnakeTiles tiles; + QVariantList tileCenterPoints; + SnakeTilesLocal tilesENU; + QVector tileCenterPointsENU; - - // Output - std::atomic_bool calcInProgress; - QNemoProgress qProgress; - std::vector progress; - QNemoHeartbeat heartbeat; - + // Waypoints QVector waypoints; QVector arrivalPath; QVector returnPath; - QGeoCoordinate ENUorigin; QVector waypointsENU; QVector arrivalPathENU; QVector returnPathENU; - SnakeTiles tiles; - QVariantList tileCenterPoints; - SnakeTilesLocal tilesENU; - QVector tileCenterPointsENU; QString errorMessage; + + + // Other + std::atomic_bool calcInProgress; + QNemoProgress qProgress; + std::vector progress; + QNemoHeartbeat heartbeat; + SnakeDataManager *parent; + }; template @@ -107,7 +110,7 @@ private: SnakeDataManager::SnakeDataManager(QObject *parent) : QThread(parent) - , pImpl(std::make_unique()) + , pImpl(std::make_unique(this)) { } @@ -135,16 +138,16 @@ void SnakeDataManager::setCorridor(const QList &corridor) this->pImpl->corridor = corridor; } -QNemoProgress SnakeDataManager::progress() +QNemoProgress SnakeDataManager::nemoProgress() { SharedLock lk(this->pImpl->mutex); return this->pImpl->qProgress; } -QNemoHeartbeat SnakeDataManager::heartbeat() +int SnakeDataManager::nemoStatus() { SharedLock lk(this->pImpl->mutex); - return this->pImpl->heartbeat; + return this->pImpl->heartbeat.status(); } bool SnakeDataManager::calcInProgress() @@ -200,7 +203,17 @@ void SnakeDataManager::setTileWidth(Length tileWidth) this->pImpl->scenario.setTileWidth(tileWidth); } -bool SnakeDataManager::precondition() const +void SnakeDataManager::enableRosBridge() +{ + this->pImpl->rosBridgeEnabeled = true; +} + +void SnakeDataManager::disableRosBride() +{ + this->pImpl->rosBridgeEnabeled = false; +} + +bool SnakeImpl::precondition() const { return true; } @@ -208,20 +221,20 @@ bool SnakeDataManager::precondition() const //! //! \brief Resets waypoint data. //! \pre this->_pImpl->mutex must be locked. -void SnakeDataManager::resetWaypointData() +void SnakeImpl::resetWaypointData() { - this->pImpl->waypoints.clear(); - this->pImpl->arrivalPath.clear(); - this->pImpl->returnPath.clear(); - this->pImpl->ENUorigin = QGeoCoordinate(0.0,0.0,0.0); - this->pImpl->waypointsENU.clear(); - this->pImpl->arrivalPathENU.clear(); - this->pImpl->returnPathENU.clear(); - this->pImpl->tiles.polygons().clear(); - this->pImpl->tileCenterPoints.clear(); - this->pImpl->tilesENU.polygons().clear(); - this->pImpl->tileCenterPointsENU.clear(); - this->pImpl->errorMessage.clear(); + this->waypoints.clear(); + this->arrivalPath.clear(); + this->returnPath.clear(); + this->ENUOrigin = QGeoCoordinate(0.0,0.0,0.0); + this->waypointsENU.clear(); + this->arrivalPathENU.clear(); + this->returnPathENU.clear(); + this->tiles.polygons().clear(); + this->tileCenterPoints.clear(); + this->tilesENU.polygons().clear(); + this->tileCenterPointsENU.clear(); + this->errorMessage.clear(); } void SnakeDataManager::run() @@ -230,23 +243,23 @@ void SnakeDataManager::run() ToggleRAII tr(this->pImpl->calcInProgress); UniqueLock lk(this->pImpl->mutex); - resetWaypointData(); + this->pImpl->resetWaypointData(); - if ( !precondition() ) + if ( !this->pImpl->precondition() ) return; if ( this->pImpl->mArea.size() < 3) { - pImpl->errorMessage = "Measurement area invalid: size < 3."; + this->pImpl->errorMessage = "Measurement area invalid: size < 3."; return; } if ( this->pImpl->sArea.size() < 3) { - pImpl->errorMessage = "Service area invalid: size < 3."; + this->pImpl->errorMessage = "Service area invalid: size < 3."; return; } - this->pImpl->ENUorigin = this->pImpl->mArea.front(); - auto &origin = this->pImpl->ENUorigin; + this->pImpl->ENUOrigin = this->pImpl->mArea.front(); + auto &origin = this->pImpl->ENUOrigin; // Update measurement area. auto &mAreaEnu = this->pImpl->scenario.measurementArea(); auto &mArea = this->pImpl->mArea; @@ -278,17 +291,19 @@ void SnakeDataManager::run() } if ( !this->pImpl->scenario.update() ){ - pImpl->errorMessage = this->pImpl->scenario.errorString.c_str(); + this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str(); return; } // Asynchronously update flightplan. qWarning() << "route calculation missing"; std::string errorString; - auto future = std::async([this, &errorString]{ - auto alpha = -this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree; + auto future = std::async([this, &errorString, &origin]{ + snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree); snake::flight_plan::Transects transects; - transects.push_back(bg::model::linestring{this->pImpl->scenario.homePositon()}); + transects.push_back(bg::model::linestring{ + this->pImpl->scenario.homePositon() + }); bool value = snake::flight_plan::transectsFromScenario(this->pImpl->lineDistance, this->pImpl->minTransectLength, alpha, @@ -308,29 +323,44 @@ void SnakeDataManager::run() if ( !value ) return value; - // Store waypoints. - for (auto boostVertex : _pFlightplan->waypoints()){ + // Store arrival path. + const auto &firstWaypoint = transectsRouted.front().front(); + long startIdx = 0; + for (long i = 0; i < route.size(); ++i){ + const auto &boostVertex = route[i]; + if ( boostVertex == firstWaypoint){ + startIdx = i; + break; + } QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()}; QGeoCoordinate geoVertex; snake::fromENU(origin, boostVertex, geoVertex); - pImpl->waypointsENU.push_back(enuVertex); - pImpl->waypoints.push_back(geoVertex); + this->pImpl->arrivalPathENU.push_back(enuVertex); + this->pImpl->arrivalPath.push_back(geoVertex); } - // Store arrival path. - for (auto boostVertex : _pFlightplan->arrivalPath()){ + // Store return path. + long endIdx = 0; + const auto &lastWaypoint = transectsRouted.back().back(); + for (long i = route.size()-1; i >= 0; --i){ + const auto &boostVertex = route[i]; + if ( boostVertex == lastWaypoint){ + endIdx = i; + break; + } QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()}; QGeoCoordinate geoVertex; snake::fromENU(origin, boostVertex, geoVertex); - pImpl->arrivalPathENU.push_back(enuVertex); - pImpl->arrivalPath.push_back(geoVertex); + this->pImpl->returnPathENU.push_back(enuVertex); + this->pImpl->returnPath.push_back(geoVertex); } - // Store return path. - for (auto boostVertex : _pFlightplan->returnPath()){ + // Store waypoints. + for (long i = startIdx; i <= endIdx; ++i){ + const auto &boostVertex = route[i]; QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()}; QGeoCoordinate geoVertex; snake::fromENU(origin, boostVertex, geoVertex); - pImpl->returnPathENU.push_back(enuVertex); - pImpl->returnPath.push_back(geoVertex); + this->pImpl->waypointsENU.push_back(enuVertex); + this->pImpl->waypoints.push_back(geoVertex); } return true; @@ -359,10 +389,10 @@ void SnakeDataManager::run() QGeoCoordinate geoVertex; snake::fromENU(origin, boostPoint, geoVertex); geoTile.setCenter(geoVertex); - pImpl->tiles.polygons().push_back(geoTile); - pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex)); - pImpl->tilesENU.polygons().push_back(enuTile); - pImpl->tileCenterPointsENU.push_back(enuVertex); + this->pImpl->tiles.polygons().push_back(geoTile); + this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex)); + this->pImpl->tilesENU.polygons().push_back(enuTile); + this->pImpl->tileCenterPointsENU.push_back(enuVertex); } } @@ -370,13 +400,187 @@ void SnakeDataManager::run() // Trying to generate flight plan. if ( !future.get() ){ // error - for (auto c : _pFlightplan->errorString){ - pImpl->errorMessage.push_back(QChar(c)); - } + this->pImpl->errorMessage = errorString.c_str(); } else { // Success!!! } } +bool SnakeImpl::doTopicServiceSetup() +{ + using namespace ros_bridge::messages; + UniqueLock lk(this->mutex); + + if ( this->tilesENU.polygons().size() == 0) + return false; + + // Publish snake origin. + this->pRosBridge->advertiseTopic("/snake/origin", + geographic_msgs::geo_point::messageType().c_str()); + JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); + bool ret = geographic_msgs::geo_point::toJson( + this->ENUOrigin, *jOrigin, jOrigin->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + + + // Publish snake tiles. + this->pRosBridge->advertiseTopic("/snake/tiles", + jsk_recognition_msgs::polygon_array::messageType().c_str()); + JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); + ret = jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); + + + // Subscribe nemo progress. + this->pRosBridge->subscribe("/nemo/progress", + /* callback */ [this](JsonDocUPtr pDoc){ + UniqueLock lk(this->mutex); + int requiredSize = this->tilesENU.polygons().size(); + auto& progressMsg = this->qProgress; + if ( !nemo_msgs::progress::fromJson(*pDoc, progressMsg) + || progressMsg.progress().size() != requiredSize ) { // Some error occured. + // Set progress to default. + progressMsg.progress().fill(0, requiredSize); + + // Publish snake origin. + JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); + bool ret = geographic_msgs::geo_point::toJson( + this->ENUOrigin, *jOrigin, jOrigin->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + + // Publish snake tiles. + JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); + ret = jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); + } + this->progress.clear(); + for (const auto&p : progressMsg.progress()){ + this->progress.push_back(p); + } + lk.unlock(); + + emit this->parent->nemoProgressChanged(); + }); + + + // Subscribe /nemo/heartbeat. + this->pRosBridge->subscribe("/nemo/heartbeat", + /* callback */ [this](JsonDocUPtr pDoc){ + UniqueLock lk(this->mutex); + if ( this->timeout.isActive() ) { + this->timeout.stop(); + } + auto &heartbeatMsg = this->heartbeat; + if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg) ) { + heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat)); + } + this->timeout.singleShot(TIMEOUT, [this]{ + UniqueLock lk(this->mutex); + this->heartbeat.setStatus(integral(NemoStatus::Timeout)); + emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout)); + }); + emit this->parent->nemoStatusChanged(heartbeatMsg.status()); + }); + + + // Advertise /snake/get_origin. + this->pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin", + [this](JsonDocUPtr) -> JsonDocUPtr + { + using namespace ros_bridge::messages; + SharedLock lk(this->mutex); + + JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); + auto &origin = this->ENUOrigin; + rapidjson::Value jOrigin(rapidjson::kObjectType); + bool ret = geographic_msgs::geo_point::toJson( + origin, jOrigin, pDoc->GetAllocator()); + lk.unlock(); + Q_ASSERT(ret); + (void)ret; + pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); + return pDoc; + }); + + + // Advertise /snake/get_tiles. + this->pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles", + [this](JsonDocUPtr) -> JsonDocUPtr + { + SharedLock lk(this->mutex); + + JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); + rapidjson::Value jSnakeTiles(rapidjson::kObjectType); + bool ret = jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, jSnakeTiles, pDoc->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); + return pDoc; + }); + + return true; +} + + +SnakeImpl::SnakeImpl(SnakeDataManager *p) : + rosBridgeEnabeled(false) + , topicServiceSetupDone(false) + , lineDistance(1*si::meter) + , minTransectLength(1*si::meter) + , calcInProgress(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->eventTimer, &QTimer::timeout, [this]{ + if ( this->rosBridgeEnabeled ) { + if ( !this->pRosBridge->isRunning()) { + this->pRosBridge->start(); + } else if ( this->pRosBridge->isRunning() + && this->pRosBridge->connected() + && !this->topicServiceSetupDone) { + if ( this->doTopicServiceSetup() ) + this->topicServiceSetupDone = true; + } else if ( this->pRosBridge->isRunning() + && !this->pRosBridge->connected() + && this->topicServiceSetupDone){ + this->pRosBridge->reset(); + this->pRosBridge->start(); + this->topicServiceSetupDone = false; + } + } else if ( this->pRosBridge->isRunning() ) { + this->pRosBridge->reset(); + this->topicServiceSetupDone = false; + } + }); + this->eventTimer.start(EVENT_TIMER_INTERVAL); +} diff --git a/src/Wima/Snake/SnakeDataManager.h b/src/Wima/Snake/SnakeDataManager.h index d26d2151f..795a7a3a5 100644 --- a/src/Wima/Snake/SnakeDataManager.h +++ b/src/Wima/Snake/SnakeDataManager.h @@ -8,7 +8,7 @@ #include "QNemoProgress.h" #include "QNemoHeartbeat.h" -#include > +#include using namespace boost::units; @@ -17,6 +17,13 @@ using Area = quantity; class SnakeImpl; +enum class NemoStatus{ + NotConnected = 0, + Connected = 1, + Timeout = -1, + InvalidHeartbeat = -2 +}; + class SnakeDataManager : public QThread{ Q_OBJECT @@ -31,8 +38,8 @@ public: void setServiceArea (const QList &serviceArea); void setCorridor (const QList &corridor); - QNemoProgress progress(); - QNemoHeartbeat heartbeat(); + QNemoProgress nemoProgress(); + int nemoStatus(); bool calcInProgress(); Length lineDistance() const; @@ -50,14 +57,17 @@ public: Length tileWidth() const; void setTileWidth(Length tileWidth); + void enableRosBridge(); + void disableRosBride(); +signals: + void nemoProgressChanged(); + void nemoStatusChanged(int status); + void calcInProgressChanged(bool inProgress); protected: void run() override; - private: - bool precondition() const; - void resetWaypointData(); SnakeImplPtr pImpl; }; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index d1d4aa53d..ed259d242 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -2,14 +2,6 @@ #include "utilities.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" -#include "ros_bridge/rapidjson/include/rapidjson/writer.h" -#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.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/progress.h" -#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h" - #include "Snake/QNemoProgress.h" #include "Snake/QNemoHeartbeat.h" @@ -45,10 +37,10 @@ const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLengt WimaController::StatusMap WimaController::_nemoStatusMap{ std::make_pair(0, "No Heartbeat"), std::make_pair(1, "Connected"), - std::make_pair(-1, "Timeout")}; + std::make_pair(-1, "Timeout"), + std::make_pair(-2, "Error")}; using namespace snake; -using namespace snake_geometry; WimaController::WimaController(QObject *parent) : QObject (parent) @@ -125,11 +117,19 @@ WimaController::WimaController(QObject *parent) // Snake Worker Thread. connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults); connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); - connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeWorker::quit); + connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeDataManager::quit); // Snake. - connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker); - _initStartSnakeWorker(); + auto configRosBridge = [this]{ + if ( this->_enableSnake.rawValue().toBool() ){ + this->_snakeDataManager.enableRosBridge(); + } else { + this->_snakeDataManager.disableRosBride(); + } + }; + connect(&_enableSnake, &Fact::rawValueChanged, configRosBridge); + configRosBridge(); + connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager); _switchSnakeManager(_enableSnake.rawValue()); } @@ -202,8 +202,9 @@ Fact *WimaController::altitude() { QmlObjectListModel *WimaController::snakeTiles() { + static QmlObjectListModel list; qWarning() << "using snake tile dummy"; - return QmlObjectListModel(); + return &list; } QVariantList WimaController::snakeTileCenterPoints() @@ -212,11 +213,10 @@ QVariantList WimaController::snakeTileCenterPoints() return QVariantList(); } -QVector WimaController::nemoProgress() { - if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() ) - return _nemoProgress.progress(); - else - return QVector(_snakeTileCenterPoints.size(), 0); +QVector WimaController::nemoProgress() +{ + qWarning() << "using nemoProgress dummy"; + return QVector(); } double WimaController::phaseDistance() const @@ -231,17 +231,19 @@ double WimaController::phaseDuration() const int WimaController::nemoStatus() const { - return _nemoHeartbeat.status(); + qWarning() << "using nemoStatus dummy"; + return 0; } QString WimaController::nemoStatusString() const { - return _nemoStatusMap.at(_nemoHeartbeat.status()); + return _nemoStatusMap.at(nemoStatus()); } bool WimaController::snakeCalcInProgress() const { - return _snakeCalcInProgress; + qWarning() << "using snakeCalcInProgress dummy"; + return false; } void WimaController::setMasterController(PlanMasterController *masterC) @@ -762,28 +764,6 @@ void WimaController::_eventTimerHandler() emit WimaController::nemoStatusChanged(); emit WimaController::nemoStatusStringChanged(); } - - if ( _snakeTicker.ready() ) { - if ( _enableSnake.rawValue().toBool() ) { - if ( !_pRosBridge->isRunning()) { - _pRosBridge->start(); - } else if ( _pRosBridge->isRunning() - && _pRosBridge->connected() - && !_topicServiceSetupDone) { - if ( _doTopicServiceSetup() ) - _topicServiceSetupDone = true; - } else if ( _pRosBridge->isRunning() - && !_pRosBridge->connected() - && _topicServiceSetupDone){ - _pRosBridge->reset(); - _pRosBridge->start(); - _topicServiceSetupDone = false; - } - } else if ( _pRosBridge->isRunning() ) { - _pRosBridge->reset(); - _topicServiceSetupDone = false; - } - } } void WimaController::_smartRTLCleanUp(bool flying) @@ -965,114 +945,4 @@ void WimaController::_switchSnakeManager(QVariant variant) } } -bool WimaController::_doTopicServiceSetup() -{ - using namespace ros_bridge::messages; - - if ( _snakeTilesLocal.polygons().size() == 0) - return false; - - // Publish snake origin. - _pRosBridge->advertiseTopic("/snake/origin", - geographic_msgs::geo_point::messageType().c_str()); - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - bool ret = geographic_msgs::geo_point::toJson( - _snakeOrigin, *jOrigin.get(), jOrigin->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - _pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - - - // Publish snake tiles. - _pRosBridge->advertiseTopic("/snake/tiles", - jsk_recognition_msgs::polygon_array::messageType().c_str()); - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - ret = jsk_recognition_msgs::polygon_array::toJson( - _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - - - // Subscribe nemo progress. - _pRosBridge->subscribe("/nemo/progress", - /* callback */ [this](JsonDocUPtr pDoc){ - int requiredSize = this->_snakeTilesLocal.polygons().size(); - auto& progress_msg = this->_nemoProgress; - if ( !nemo_msgs::progress::fromJson(*pDoc, progress_msg) - || progress_msg.progress().size() != requiredSize ) { // Some error occured. - // Set progress to default. - progress_msg.progress().fill(0, requiredSize); - - // Publish snake origin. - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - bool ret = geographic_msgs::geo_point::toJson( - this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - - // Publish snake tiles. - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - ret = jsk_recognition_msgs::polygon_array::toJson( - this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - } - - emit WimaController::nemoProgressChanged(); - }); - - - // Subscribe /nemo/heartbeat. - _pRosBridge->subscribe("/nemo/heartbeat", - /* callback */ [this](JsonDocUPtr pDoc){ - auto &heartbeat_msg = this->_nemoHeartbeat; - if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeat_msg) ) { - if ( heartbeat_msg.status() == this->_fallbackStatus ) - return; - heartbeat_msg.setStatus(this->_fallbackStatus); - } - this->_nemoTimeoutTicker.reset(); - this->_fallbackStatus = -1; /*Timeout*/ - emit WimaController::nemoStatusChanged(); - emit WimaController::nemoStatusStringChanged(); - }); - - - // Advertise /snake/get_origin. - _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin", - [this](JsonDocUPtr) -> JsonDocUPtr - { - using namespace ros_bridge::messages; - - JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); - ::GeoPoint3D origin = this->_snakeOrigin; - rapidjson::Value jOrigin(rapidjson::kObjectType); - bool ret = geographic_msgs::geo_point::toJson( - origin, jOrigin, pDoc->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); - return pDoc; - }); - - - // Advertise /snake/get_tiles. - _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles", - [this](JsonDocUPtr) -> JsonDocUPtr - { - JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); - rapidjson::Value jSnakeTiles(rapidjson::kObjectType); - bool ret = jsk_recognition_msgs::polygon_array::toJson( - this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); - return pDoc; - }); - - return true; -} -- 2.22.0