#include "SnakeDataManager.h" #include #include #include "QGCApplication.h" #include "QGCToolbox.h" #include "SettingsFact.h" #include "SettingsManager.h" #include "WimaSettings.h" #include #include #include #include "Wima/Snake/SnakeTiles.h" #include "Wima/Snake/SnakeTilesLocal.h" #include "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" #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 SnakeDataManager::SnakeImpl { public: 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); } bool precondition() const; void resetOutput(); bool doTopicServiceSetup(); // Private data. 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; QmlObjectListModel tilesQml; QVariantList tileCenterPoints; SnakeTilesLocal tilesENU; QVector tileCenterPointsENU; // Waypoints QVector waypoints; QVector arrivalPath; QVector returnPath; QVector waypointsENU; QVector arrivalPathENU; QVector returnPathENU; QString errorMessage; // Other std::atomic_bool calcInProgress; QNemoProgress qProgress; std::vector progress; QNemoHeartbeat heartbeat; SnakeDataManager *parent; }; bool SnakeDataManager::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; } bool SnakeDataManager::SnakeImpl::precondition() const { return true; } //! //! \brief Resets waypoint data. //! \pre this->_pImpl->mutex must be locked. void SnakeDataManager::SnakeImpl::resetOutput() { 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->tilesQml.clearAndDeleteContents(); this->tiles.polygons().clear(); this->tileCenterPoints.clear(); this->tilesENU.polygons().clear(); this->tileCenterPointsENU.clear(); this->errorMessage.clear(); } template class CommandRAII { public: CommandRAII(Callable &fun) : _fun(fun) {} ~CommandRAII() { _fun(); } private: Callable _fun; }; SnakeDataManager::SnakeDataManager(QObject *parent) : QThread(parent), pImpl(std::make_unique(this)) {} SnakeDataManager::~SnakeDataManager() {} void SnakeDataManager::setMeasurementArea( const QList &measurementArea) { UniqueLock lk(this->pImpl->mutex); this->pImpl->mArea = measurementArea; } void SnakeDataManager::setServiceArea( const QList &serviceArea) { UniqueLock lk(this->pImpl->mutex); this->pImpl->sArea = serviceArea; } void SnakeDataManager::setCorridor(const QList &corridor) { UniqueLock lk(this->pImpl->mutex); this->pImpl->corridor = corridor; } const QmlObjectListModel *SnakeDataManager::tiles() const { SharedLock lk(this->pImpl->mutex); return &this->pImpl->tilesQml; } QVariantList SnakeDataManager::tileCenterPoints() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->tileCenterPoints; } QNemoProgress SnakeDataManager::nemoProgress() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->qProgress; } int SnakeDataManager::nemoStatus() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->heartbeat.status(); } bool SnakeDataManager::calcInProgress() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->calcInProgress.load(); } QString SnakeDataManager::errorMessage() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->errorMessage; } bool SnakeDataManager::success() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->errorMessage.isEmpty() ? true : false; } QVector SnakeDataManager::waypoints() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->waypoints; } QVector SnakeDataManager::arrivalPath() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->arrivalPath; } QVector SnakeDataManager::returnPath() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->returnPath; } Length SnakeDataManager::lineDistance() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->lineDistance; } void SnakeDataManager::setLineDistance(Length lineDistance) { UniqueLock lk(this->pImpl->mutex); this->pImpl->lineDistance = lineDistance; } Area SnakeDataManager::minTileArea() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->scenario.minTileArea(); } void SnakeDataManager::setMinTileArea(Area minTileArea) { UniqueLock lk(this->pImpl->mutex); this->pImpl->scenario.setMinTileArea(minTileArea); } Length SnakeDataManager::tileHeight() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->scenario.tileHeight(); } void SnakeDataManager::setTileHeight(Length tileHeight) { UniqueLock lk(this->pImpl->mutex); this->pImpl->scenario.setTileHeight(tileHeight); } Length SnakeDataManager::tileWidth() const { SharedLock lk(this->pImpl->mutex); return this->pImpl->scenario.tileWidth(); } void SnakeDataManager::setTileWidth(Length tileWidth) { UniqueLock lk(this->pImpl->mutex); this->pImpl->scenario.setTileWidth(tileWidth); } void SnakeDataManager::enableRosBridge() { this->pImpl->rosBridgeEnabeled = true; } void SnakeDataManager::disableRosBride() { this->pImpl->rosBridgeEnabeled = false; } void SnakeDataManager::run() { this->pImpl->calcInProgress.store(true); emit calcInProgressChanged(this->pImpl->calcInProgress.load()); auto onExit = [this] { this->pImpl->calcInProgress.store(false); emit calcInProgressChanged(this->pImpl->calcInProgress.load()); }; CommandRAII onExitRAII(onExit); UniqueLock lk(this->pImpl->mutex); this->pImpl->resetOutput(); if (!this->pImpl->precondition()) return; if (this->pImpl->mArea.size() < 3) { this->pImpl->errorMessage = "Measurement area invalid: size < 3."; return; } if (this->pImpl->sArea.size() < 3) { this->pImpl->errorMessage = "Service area invalid: size < 3."; return; } 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; mAreaEnu.clear(); for (auto geoVertex : mArea) { snake::BoostPoint p; snake::toENU(origin, geoVertex, p); mAreaEnu.outer().push_back(p); } // Update service area. auto &sAreaEnu = this->pImpl->scenario.serviceArea(); auto &sArea = this->pImpl->sArea; sAreaEnu.clear(); for (auto geoVertex : sArea) { snake::BoostPoint p; snake::toENU(origin, geoVertex, p); sAreaEnu.outer().push_back(p); } // Update corridor. auto &corridorEnu = this->pImpl->scenario.corridor(); auto &corridor = this->pImpl->corridor; corridor.clear(); for (auto geoVertex : corridor) { snake::BoostPoint p; snake::toENU(origin, geoVertex, p); corridorEnu.outer().push_back(p); } if (!this->pImpl->scenario.update()) { this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str(); return; } // Asynchronously update flightplan. std::string errorString; 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()}); bool value = snake::flight_plan::transectsFromScenario( this->pImpl->lineDistance, this->pImpl->minTransectLength, alpha, this->pImpl->scenario, this->pImpl->progress, transects, errorString); if (!value) { this->pImpl->errorMessage = "Not able to generate transects."; return value; } snake::flight_plan::Transects transectsRouted; snake::flight_plan::Route route; value = snake::flight_plan::route(this->pImpl->scenario.joinedArea(), transects, transectsRouted, route, errorString); if (!value) { this->pImpl->errorMessage = "Routing error."; return value; } // Store arrival path. const auto &firstWaypoint = transectsRouted.front().front(); long startIdx = 0; for (long i = 0; i < long(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); this->pImpl->arrivalPathENU.push_back(enuVertex); this->pImpl->arrivalPath.push_back(geoVertex); } // 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); this->pImpl->returnPathENU.push_back(enuVertex); this->pImpl->returnPath.push_back(geoVertex); } // 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); this->pImpl->waypointsENU.push_back(enuVertex); this->pImpl->waypoints.push_back(geoVertex); } return true; }); // Continue with storing scenario data in the mean time. { // Get tiles. const auto &tiles = this->pImpl->scenario.tiles(); const auto ¢erPoints = this->pImpl->scenario.tileCenterPoints(); for (unsigned int i = 0; i < tiles.size(); ++i) { const auto &tile = tiles[i]; SnakeTile geoTile; SnakeTileLocal enuTile; for (size_t i = tile.outer().size(); i < tile.outer().size() - 1; ++i) { auto &p = tile.outer()[i]; QPointF enuVertex(p.get<0>(), p.get<1>()); QGeoCoordinate geoVertex; snake::fromENU(origin, p, geoVertex); enuTile.polygon().points().push_back(enuVertex); geoTile.push_back(geoVertex); } const auto &boostPoint = centerPoints[i]; QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>()); QGeoCoordinate geoVertex; snake::fromENU(origin, boostPoint, geoVertex); geoTile.setCenter(geoVertex); this->pImpl->tilesQml.append(new SnakeTile(geoTile)); 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); } } future.wait(); // Trying to generate flight plan. if (!future.get()) { // error this->pImpl->errorMessage = errorString.c_str(); } else { // Success!!! } }