diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 2b11cb8b0e56a43351d472b682eef694aad79c08..274f0c0a49f57315cd4f973ebabd62eb12149a6c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -489,7 +489,7 @@ HEADERS += \ src/comm/ros_bridge/include/MessageTag.h \ src/comm/ros_bridge/include/MessageTraits.h \ src/comm/ros_bridge/include/RosBridgeClient.h \ - src/comm/ros_bridge/include/ThreadSafeQueue.h \ + src/comm/ros_bridge/include/Server.h \ src/comm/ros_bridge/include/TopicPublisher.h \ src/comm/ros_bridge/include/TopicSubscriber.h \ src/comm/ros_bridge/include/TypeFactory.h \ @@ -513,6 +513,7 @@ SOURCES += \ src/Wima/WimaBridge.cc \ src/comm/ros_bridge/include/ComPrivateInclude.cpp \ src/comm/ros_bridge/include/MessageTag.cpp \ + src/comm/ros_bridge/include/Server.cpp \ src/comm/ros_bridge/include/TopicPublisher.cpp \ src/comm/ros_bridge/include/TopicSubscriber.cpp \ src/comm/ros_bridge/src/CasePacker.cpp \ diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index a213b2ff1993d41a21bfd69fc14141086ffe5b21..c13aae906502e405382e591f5899e8ea3bb1e44a 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -35,9 +35,10 @@ const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea"; const char* WimaController::snakeLineDistanceName = "SnakeLineDistance"; const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLength"; -WimaController::StatusMap WimaController::_nemoStatusMap{std::make_pair(0, "No Heartbeat"), - std::make_pair(1, "Connected"), - std::make_pair(-1, "Timeout")}; +WimaController::StatusMap WimaController::_nemoStatusMap{ + std::make_pair(0, "No Heartbeat"), + std::make_pair(1, "Connected"), + std::make_pair(-1, "Timeout")}; using namespace snake; using namespace snake_geometry; @@ -55,7 +56,9 @@ WimaController::WimaController(QObject *parent) , _snakeManager (_managerSettings, _areaInterface) , _rtlManager (_managerSettings, _areaInterface) , _currentManager (&_defaultManager) - , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) + , _managerList {&_defaultManager, &_snakeManager, &_rtlManager} + , _metaDataMap (FactMetaData::createMapFromJsonFile( + QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) , _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName]) , _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName]) , _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName]) @@ -68,7 +71,6 @@ WimaController::WimaController(QObject *parent) , _measurementPathLength (-1) , _lowBatteryHandlingTriggered (false) , _snakeCalcInProgress (false) - , _scenarioDefinedBool (false) , _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName]) , _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName]) , _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName]) @@ -89,9 +91,20 @@ WimaController::WimaController(QObject *parent) connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed); connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude); - _defaultManager.setOverlap(_overlapWaypoints.rawValue().toUInt()); - _defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt()); - _defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt()); + // Init waypoint managers. + bool value; + size_t overlap = _overlapWaypoints.rawValue().toUInt(&value); + Q_ASSERT(value); + size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value); + Q_ASSERT(value); + size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value); + Q_ASSERT(value); + (void)value; + for (auto manager : _managerList){ + manager->setOverlap(overlap); + manager->setN(N); + manager->setStartIndex(startIdx); + } // Periodic. connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler); @@ -343,17 +356,8 @@ bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCo return retVal; } -/*! - * \fn void WimaController::containerDataValidChanged(bool valid) - * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true). - * Is connected to the dataValidChanged() signal of the \c WimaDataContainer. - * - * \sa WimaDataContainer, WimaPlaner, WimaPlanData - */ bool WimaController::setWimaPlanData(const WimaPlanData &planData) { - // fetch only if valid, return true on success - // reset visual items _areas.clear(); _defaultManager.clear(); @@ -779,17 +783,6 @@ void WimaController::_eventTimerHandler() _pRosBridge->reset(); _bridgeSetupDone = false; } - - //qWarning() << "Connectable: " << _pRosBridge->connected() << "\n"; - - - //auto start = std::chrono::high_resolution_clock::now(); - - - - //auto end = std::chrono::high_resolution_clock::now(); - //qWarning() << "Duration: " << std::chrono::duration_cast(end-start).count() - //<< " ms\n"; } } @@ -838,14 +831,23 @@ void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manage if (_currentManager != &manager) { _currentManager = &manager; - bool value; - _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value)); - Q_ASSERT(value); - _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value)); - Q_ASSERT(value); - _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1); - Q_ASSERT(value); - (void)value; + disconnect(&_overlapWaypoints, &Fact::rawValueChanged, + this, &WimaController::_updateOverlap); + disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, + this, &WimaController::_updateMaxWaypoints); + disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, + this, &WimaController::_setStartIndex); + + _maxWaypointsPerPhase.setRawValue(_currentManager->N()); + _overlapWaypoints.setRawValue(_currentManager->overlap()); + _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex()); + + connect(&_overlapWaypoints, &Fact::rawValueChanged, + this, &WimaController::_updateOverlap); + connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, + this, &WimaController::_updateMaxWaypoints); + connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, + this, &WimaController::_setStartIndex); emit missionItemsChanged(); emit currentMissionItemsChanged(); @@ -888,33 +890,13 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress) } } -bool WimaController::_isScenarioDefined() -{ - _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(), - _snakeTileHeight.rawValue().toDouble(), - _snakeMinTileArea.rawValue().toDouble()); - return _scenarioDefinedBool; -} - -bool WimaController::_isScenarioDefinedErrorMessage() -{ - bool value = _isScenarioDefined(); - if (!value){ - QString errorString; - for (auto c : _scenario.errorString) - errorString.push_back(c); - qgcApp()->showMessage(errorString); - } - return value; -} - void WimaController::_snakeStoreWorkerResults() { + _setSnakeCalcInProgress(false); auto start = std::chrono::high_resolution_clock::now(); _snakeManager.clear(); const WorkerResult_t &r = _snakeWorker.getResult(); - _setSnakeCalcInProgress(false); if (!r.success) { //qgcApp()->showMessage(r.errorMessage); return; diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 6fd11ca0fdb1bc95dccb8f1cda268162d478cc9d..063c13a7b1d319673eb50281ac59b8ff47e0d283 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -338,8 +338,6 @@ private slots: void _smartRTLCleanUp (bool flying); // Snake. void _setSnakeCalcInProgress (bool inProgress); - bool _isScenarioDefined (void); - bool _isScenarioDefinedErrorMessage (void); void _snakeStoreWorkerResults (); void _startStopRosBridge (); void _initStartSnakeWorker (); @@ -357,7 +355,6 @@ private: MissionController *_missionController; // Wima Data. - QmlObjectListModel _areas; // contains all visible areas WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor WimaMeasurementAreaData _measurementArea; // measurement area @@ -372,6 +369,8 @@ private: WaypointManager::DefaultManager _snakeManager; WaypointManager::RTLManager _rtlManager; WaypointManager::ManagerBase *_currentManager; + using ManagerList = QList; + ManagerList _managerList; // Settings Facts. QMap _metaDataMap; diff --git a/src/Wima/WimaController_new.cc b/src/Wima/WimaController_new.cc new file mode 100644 index 0000000000000000000000000000000000000000..24bf234c533a7ba4cdcdc17125df9144ab177d13 --- /dev/null +++ b/src/Wima/WimaController_new.cc @@ -0,0 +1,924 @@ +#include "WimaController.h" +#include "utilities.h" +#include "ros_bridge/include/JsonMethodes.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/CasePacker.h" + +#include "Snake/QtROSJsonFactory.h" +#include "Snake/QtROSTypeFactory.h" +#include "Snake/QNemoProgress.h" +#include "Snake/QNemoHeartbeat.h" + +#include "QVector3D" +#include + + + +// const char* WimaController::wimaFileExtension = "wima"; +const char* WimaController::areaItemsName = "AreaItems"; +const char* WimaController::missionItemsName = "MissionItems"; +const char* WimaController::settingsGroup = "WimaController"; +const char* WimaController::enableWimaControllerName = "EnableWimaController"; +const char* WimaController::overlapWaypointsName = "OverlapWaypoints"; +const char* WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase"; +const char* WimaController::startWaypointIndexName = "StartWaypointIndex"; +const char* WimaController::showAllMissionItemsName = "ShowAllMissionItems"; +const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems"; +const char* WimaController::flightSpeedName = "FlightSpeed"; +const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed"; +const char* WimaController::altitudeName = "Altitude"; +const char* WimaController::snakeTileWidthName = "SnakeTileWidth"; +const char* WimaController::snakeTileHeightName = "SnakeTileHeight"; +const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea"; +const char* WimaController::snakeLineDistanceName = "SnakeLineDistance"; +const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLength"; + +WimaController::StatusMap WimaController::_nemoStatusMap{ + std::make_pair(0, "No Heartbeat"), + std::make_pair(1, "Connected"), + std::make_pair(-1, "Timeout")}; + +using namespace snake; +using namespace snake_geometry; + +WimaController::WimaController(QObject *parent) + : QObject (parent) + , _joinedArea () + , _measurementArea () + , _serviceArea () + , _corridor () + , _localPlanDataValid (false) + , _areaInterface (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea) + , _managerSettings () + , _defaultManager (_managerSettings, _areaInterface) + , _snakeManager (_managerSettings, _areaInterface) + , _rtlManager (_managerSettings, _areaInterface) + , _currentManager (&_defaultManager) + , _managerList {&_defaultManager, &_snakeManager, &_rtlManager} + , _metaDataMap (FactMetaData::createMapFromJsonFile( + QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) + , _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName]) + , _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName]) + , _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName]) + , _nextPhaseStartWaypointIndex (settingsGroup, _metaDataMap[startWaypointIndexName]) + , _showAllMissionItems (settingsGroup, _metaDataMap[showAllMissionItemsName]) + , _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName]) + , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) + , _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName]) + , _altitude (settingsGroup, _metaDataMap[altitudeName]) + , _measurementPathLength (-1) + , _lowBatteryHandlingTriggered (false) + , _snakeCalcInProgress (false) + , _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName]) + , _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName]) + , _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName]) + , _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName]) + , _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName]) + , _nemoHeartbeat (0 /*status: not connected*/) + , _fallbackStatus (0 /*status: not connected*/) + , _bridgeSetupDone (false) + , _pRosBridge (new ROSBridge::ROSBridge()) +{ + // Set up facts. + _showAllMissionItems.setRawValue(true); + _showCurrentMissionItems.setRawValue(true); + connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateOverlap); + connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints); + connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_setStartIndex); + connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed); + connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed); + connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude); + + // Init waypoint managers. + bool value; + size_t overlap = _overlapWaypoints.rawValue().toUInt(&value); + Q_ASSERT(value); + size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value); + Q_ASSERT(value); + size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value); + Q_ASSERT(value); + (void)value; + for (auto manager : _managerList){ + manager->setOverlap(overlap); + manager->setN(N); + manager->setStartIndex(startIdx); + } + + // Periodic. + connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler); + //_eventTimer.setInterval(EVENT_TIMER_INTERVAL); + _eventTimer.start(EVENT_TIMER_INTERVAL); + + // Snake Worker Thread. + connect(&_snakeWorker, &SnakeDataManager::finished, this, &WimaController::_updateSnakeData); + connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); + connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeDataManager::quit); + + // Snake. + connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager); + _switchSnakeManager(_enableSnake.rawValue()); +} + +PlanMasterController *WimaController::masterController() { + return _masterController; +} + +MissionController *WimaController::missionController() { + return _missionController; +} + +QmlObjectListModel *WimaController::visualItems() { + return &_areas; +} + +QmlObjectListModel *WimaController::missionItems() { + return const_cast(&_currentManager->missionItems()); +} + +QmlObjectListModel *WimaController::currentMissionItems() { + return const_cast(&_currentManager->currentMissionItems()); +} + +QVariantList WimaController::waypointPath() +{ + return const_cast(_currentManager->waypointsVariant()); +} + +QVariantList WimaController::currentWaypointPath() +{ + return const_cast(_currentManager->currentWaypointsVariant()); +} + +Fact *WimaController::enableWimaController() { + return &_enableWimaController; +} + +Fact *WimaController::overlapWaypoints() { + return &_overlapWaypoints; +} + +Fact *WimaController::maxWaypointsPerPhase() { + return &_maxWaypointsPerPhase; +} + +Fact *WimaController::startWaypointIndex() { + return &_nextPhaseStartWaypointIndex; +} + +Fact *WimaController::showAllMissionItems() { + return &_showAllMissionItems; +} + +Fact *WimaController::showCurrentMissionItems() { + return &_showCurrentMissionItems; +} + +Fact *WimaController::flightSpeed() { + return &_flightSpeed; +} + +Fact *WimaController::arrivalReturnSpeed() { + return &_arrivalReturnSpeed; +} + +Fact *WimaController::altitude() { + return &_altitude; +} + +QVector WimaController::nemoProgress() { + if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() ) + return _nemoProgress.progress(); + else + return QVector(_snakeTileCenterPoints.size(), 0); +} + +double WimaController::phaseDistance() const +{ + return 0.0; +} + +double WimaController::phaseDuration() const +{ + return 0.0; +} + +int WimaController::nemoStatus() const +{ + return _nemoHeartbeat.status(); +} + +QString WimaController::nemoStatusString() const +{ + return _nemoStatusMap.at(_nemoHeartbeat.status()); +} + +bool WimaController::snakeCalcInProgress() const +{ + return _snakeCalcInProgress; +} + +void WimaController::setMasterController(PlanMasterController *masterC) +{ + _masterController = masterC; + _managerSettings.setMasterController(masterC); + emit masterControllerChanged(); +} + +void WimaController::setMissionController(MissionController *missionC) +{ + _missionController = missionC; + _managerSettings.setMissionController(missionC); + emit missionControllerChanged(); +} + +void WimaController::nextPhase() +{ + _calcNextPhase(); +} + +void WimaController::previousPhase() +{ + if ( !_currentManager->previous() ) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::resetPhase() +{ + if ( !_currentManager->reset() ) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::requestSmartRTL() +{ + QString errorString("Smart RTL requested. "); + if ( !_checkSmartRTLPreCondition(errorString) ){ + qgcApp()->showMessage(errorString); + return; + } + emit smartRTLRequestConfirm(); +} + +bool WimaController::upload() +{ + auto ¤tMissionItems = _defaultManager.currentMissionItems(); + if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate()) + && currentMissionItems.count() > 0) { + emit forceUploadConfirm(); + return false; + } + + return forceUpload(); +} + +bool WimaController::forceUpload() +{ + auto ¤tMissionItems = _defaultManager.currentMissionItems(); + if (currentMissionItems.count() < 1) + return false; + + _missionController->removeAll(); + // Set homeposition of settingsItem. + QmlObjectListModel* visuals = _missionController->visualItems(); + MissionSettingsItem* settingsItem = visuals->value(0); + if (settingsItem == nullptr) { + Q_ASSERT(false); + qWarning("WimaController::updateCurrentMissionItems(): nullptr"); + return false; + } + settingsItem->setCoordinate(_managerSettings.homePosition()); + + // Copy mission items to _missionController. + for (int i = 1; i < currentMissionItems.count(); i++){ + auto *item = currentMissionItems.value(i); + _missionController->insertSimpleMissionItem(*item, visuals->count()); + } + + _masterController->sendToVehicle(); + + return true; +} + +void WimaController::removeFromVehicle() +{ + _masterController->removeAllFromVehicle(); + _missionController->removeAll(); +} + +void WimaController::executeSmartRTL() +{ + forceUpload(); + masterController()->managerVehicle()->startMission(); +} + +void WimaController::initSmartRTL() +{ + _initSmartRTL(); +} + +void WimaController::removeVehicleTrajectoryHistory() +{ + Vehicle *managerVehicle = masterController()->managerVehicle(); + managerVehicle->trajectoryPoints()->clear(); +} + +bool WimaController::_calcShortestPath(const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path) +{ + using namespace GeoUtilities; + using namespace PolygonCalculus; + QPolygonF polygon2D; + toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D); + QPointF start2D(0,0); + QPointF end2D; + toCartesian(destination, start, end2D); + QVector path2D; + + bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); + toGeoList(path2D, /*origin*/ start, path); + + return retVal; +} + +bool WimaController::setWimaPlanData(const WimaPlanData &planData) +{ + // reset visual items + _areas.clear(); + _defaultManager.clear(); + _snakeTiles.polygons().clear(); + _snakeTilesLocal.polygons().clear(); + _snakeTileCenterPoints.clear(); + + emit visualItemsChanged(); + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); + emit snakeTilesChanged(); + emit snakeTileCenterPointsChanged(); + + _localPlanDataValid = false; + + // extract list with WimaAreas + QList areaList = planData.areaList(); + + int areaCounter = 0; + const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored + for (int i = 0; i < areaList.size(); i++) { + const WimaAreaData *areaData = areaList[i]; + + if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area? + _serviceArea = *qobject_cast(areaData); + areaCounter++; + _areas.append(&_serviceArea); + + continue; + } + + if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area? + _measurementArea = *qobject_cast(areaData); + areaCounter++; + _areas.append(&_measurementArea); + + continue; + } + + if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor? + _corridor = *qobject_cast(areaData); + areaCounter++; + //_visualItems.append(&_corridor); // not needed + + continue; + } + + if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor? + _joinedArea = *qobject_cast(areaData); + areaCounter++; + _areas.append(&_joinedArea); + + continue; + } + + if (areaCounter >= numAreas) + break; + } + + if (areaCounter != numAreas) { + Q_ASSERT(false); + return false; + } + + emit visualItemsChanged(); + + // extract mission items + QList tempMissionItems = planData.missionItems(); + if (tempMissionItems.size() < 1) { + qWarning("WimaController: Mission items from WimaPlaner empty!"); + return false; + } + + for (auto item : tempMissionItems) { + _defaultManager.push_back(item.coordinate()); + } + + _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(), + _serviceArea.center().longitude(), + 0) ); + + if( !_defaultManager.reset() ){ + Q_ASSERT(false); + return false; + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); + + _localPlanDataValid = true; + + _initStartSnakeWorker(); + + return true; +} + +WimaController *WimaController::thisPointer() +{ + return this; +} + +bool WimaController::_calcNextPhase() +{ + if ( !_currentManager->next() ) { + Q_ASSERT(false); + return false; + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); + + return true; +} + +bool WimaController::_setStartIndex() +{ + bool value; + _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + return false; + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); + + return true; +} + +void WimaController::_recalcCurrentPhase() +{ + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateOverlap() +{ + bool value; + _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value)); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + assert(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateMaxWaypoints() +{ + bool value; + _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value)); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateflightSpeed() +{ + bool value; + _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value)); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateArrivalReturnSpeed() +{ + bool value; + _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value)); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateAltitude() +{ + bool value; + _managerSettings.setAltitude(_altitude.rawValue().toDouble(&value)); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_checkBatteryLevel() +{ + Vehicle *managerVehicle = masterController()->managerVehicle(); + WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings(); + int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt(); + bool enabled = _enableWimaController.rawValue().toBool(); + unsigned int minTime = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt(); + + if (managerVehicle != nullptr && enabled == true) { + Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); + Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); + + + if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold + && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) { + if (!_lowBatteryHandlingTriggered) { + _lowBatteryHandlingTriggered = true; + if ( !(_missionController->remainingTime() <= minTime) ) { + requestSmartRTL(); + } + } + } + else { + _lowBatteryHandlingTriggered = false; + } + + } +} + +void WimaController::_eventTimerHandler() +{ + static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL); + static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL); + static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000); + + // Battery level check necessary? + Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling(); + if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() ) + _checkBatteryLevel(); + + // Snake flight plan update necessary? +// if ( snakeEventLoopTicker.ready() ) { +// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) { +// } +// } + + if ( nemoStatusTicker.ready() ) { + this->_nemoHeartbeat.setStatus(_fallbackStatus); + emit WimaController::nemoStatusChanged(); + emit WimaController::nemoStatusStringChanged(); + } + + if ( snakeTicker.ready() ) { + if ( _enableSnake.rawValue().toBool() + && _pRosBridge->connected() ) { + if ( !_bridgeSetupDone ) { + qWarning() << "ROS Bridge setup. "; + auto start = std::chrono::high_resolution_clock::now(); + _pRosBridge->start(); + auto end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->start() time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->publish(_snakeOrigin, "/snake/origin"); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->publish(_snakeOrigin, \"/snake/origin\") time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->publish(_snakeTilesLocal, \"/snake/tiles\") time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + + + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->subscribe("/nemo/progress", + /* callback */ [this](JsonDocUPtr pDoc){ + int requiredSize = this->_snakeTilesLocal.polygons().size(); + auto& progress = this->_nemoProgress; + if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress) + || progress.progress().size() != requiredSize ) { + progress.progress().fill(0, requiredSize); + } + + emit WimaController::nemoProgressChanged(); + }); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->subscribe(\"/nemo/progress\" time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + + start = std::chrono::high_resolution_clock::now(); + _pRosBridge->subscribe("/nemo/heartbeat", + /* callback */ [this, &nemoStatusTicker](JsonDocUPtr pDoc){ + if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) { + if ( this->_nemoHeartbeat.status() == this->_fallbackStatus ) + return; + this->_nemoHeartbeat.setStatus(this->_fallbackStatus); + } + + nemoStatusTicker.reset(); + this->_fallbackStatus = -1; /*Timeout*/ + emit WimaController::nemoStatusChanged(); + emit WimaController::nemoStatusStringChanged(); + }); + end = std::chrono::high_resolution_clock::now(); + qWarning() << "_pRosBridge->subscribe(\"/nemo/heartbeat\" time: " + << std::chrono::duration_cast(end-start).count() + << " ms"; + _bridgeSetupDone = true; + } + } else if ( _bridgeSetupDone) { + _pRosBridge->reset(); + _bridgeSetupDone = false; + } + } +} + +void WimaController::_smartRTLCleanUp(bool flying) +{ + if ( !flying) { // vehicle has landed + _switchWaypointManager(_defaultManager); + _missionController->removeAllFromVehicle(); + _missionController->removeAll(); + disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); + } +} + +void WimaController::_setPhaseDistance(double distance) +{ + (void)distance; +// if (!qFuzzyCompare(distance, _phaseDistance)) { +// _phaseDistance = distance; + +// emit phaseDistanceChanged(); +// } +} + +void WimaController::_setPhaseDuration(double duration) +{ + (void)duration; +// if (!qFuzzyCompare(duration, _phaseDuration)) { +// _phaseDuration = duration; + +// emit phaseDurationChanged(); +// } +} + +bool WimaController::_checkSmartRTLPreCondition(QString &errorString) +{ + if (!_localPlanDataValid) { + errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area.")); + return false; + } + + return _rtlManager.checkPrecondition(errorString); +} + +void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager) +{ + if (_currentManager != &manager) { + _currentManager = &manager; + + disconnect(&_overlapWaypoints, &Fact::rawValueChanged, + this, &WimaController::_updateOverlap); + disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, + this, &WimaController::_updateMaxWaypoints); + disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, + this, &WimaController::_setStartIndex); + + _maxWaypointsPerPhase.setRawValue(_currentManager->N()); + _overlapWaypoints.setRawValue(_currentManager->overlap()); + _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex()); + + connect(&_overlapWaypoints, &Fact::rawValueChanged, + this, &WimaController::_updateOverlap); + connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, + this, &WimaController::_updateMaxWaypoints); + connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, + this, &WimaController::_setStartIndex); + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); + + qWarning() << "WimaController::_switchWaypointManager: statistics update missing."; + } +} + +void WimaController::_initSmartRTL() +{ + QString errorString; + static int attemptCounter = 0; + attemptCounter++; + + if ( _checkSmartRTLPreCondition(errorString) ) { + _masterController->managerVehicle()->pauseVehicle(); + connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); + if ( _rtlManager.update() ) { // Calculate return path. + _switchWaypointManager(_rtlManager); + attemptCounter = 0; + emit smartRTLPathConfirm(); + return; + } + } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { + errorString.append(tr("Smart RTL: No success after maximum number of attempts.")); + qgcApp()->showMessage(errorString); + attemptCounter = 0; + } else { + _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL); + } +} + +void WimaController::_setSnakeCalcInProgress(bool inProgress) +{ + if (_snakeCalcInProgress != inProgress){ + _snakeCalcInProgress = inProgress; + emit snakeCalcInProgressChanged(); + } +} + +void WimaController::_updateSnakeData() +{ + _setSnakeCalcInProgress(false); + auto start = std::chrono::high_resolution_clock::now(); + _snakeManager.clear(); + const auto& r = _snakeWorker.result(); + if (!r.success) { + //qgcApp()->showMessage(r.errorMessage); + return; + } + + // create Mission items from r.waypoints + long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2; + if (n < 1) { + return; + } + + // Create QVector containing all waypoints; + unsigned long startIdx = r.arrivalPathIdx.last(); + unsigned long endIdx = r.returnPathIdx.first(); + for (unsigned long i = startIdx; i <= endIdx; ++i) { + _snakeManager.push_back(r.waypoints[int(i)]); + } + auto end = std::chrono::high_resolution_clock::now(); + double duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms."; + + + start = std::chrono::high_resolution_clock::now(); + _snakeManager.update(); + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: _snakeManager.update(); execution time: " << duration << " ms."; + + + start = std::chrono::high_resolution_clock::now(); + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: gui update execution time: " << duration << " ms."; + + start = std::chrono::high_resolution_clock::now(); + _snakeOrigin = r.origin; + _snakeTileCenterPoints = r.tileCenterPoints; + _snakeTiles = r.tiles; + _snakeTilesLocal = r.tilesLocal; + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: tiles copy execution time: " << duration << " ms."; + + start = std::chrono::high_resolution_clock::now(); + emit snakeTilesChanged(); + emit snakeTileCenterPointsChanged(); + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: gui update 2 execution time: " << duration << " ms."; +} + +void WimaController::_initStartSnakeWorker() +{ + if ( !_enableSnake.rawValue().toBool() ) + return; + + // Stop worker thread if running. + if ( _snakeWorker.isRunning() ) { + _snakeWorker.quit(); + } + + // Initialize _snakeWorker. + _snakeWorker.setMeasurementArea( + _measurementArea.coordinateList()); + _snakeWorker.setServiceArea( + _serviceArea.coordinateList()); + _snakeWorker.setCorridor( + _corridor.coordinateList()); + _snakeWorker.setProgress( + _nemoProgress.progress()); + _snakeWorker.setLineDistance( + _snakeLineDistance.rawValue().toDouble()); + _snakeWorker.setMinTransectLength( + _snakeMinTransectLength.rawValue().toDouble()); + _snakeWorker.setTileHeight( + _snakeTileHeight.rawValue().toDouble()); + _snakeWorker.setTileWidth( + _snakeTileWidth.rawValue().toDouble()); + _snakeWorker.setMinTileArea( + _snakeMinTileArea.rawValue().toDouble()); + _setSnakeCalcInProgress(true); + + // Start worker thread. + _snakeWorker.start(); +} + +void WimaController::_switchSnakeManager(QVariant variant) +{ + if (variant.value()){ + _switchWaypointManager(_snakeManager); + } else { + _switchWaypointManager(_defaultManager); + } +} diff --git a/src/Wima/WimaController_new.h b/src/Wima/WimaController_new.h new file mode 100644 index 0000000000000000000000000000000000000000..c2e49dcfadbcbb1741ad839efad1837ac8a3dbe5 --- /dev/null +++ b/src/Wima/WimaController_new.h @@ -0,0 +1,421 @@ +#pragma once + +#include +#include + +#include "QGCMapPolygon.h" +#include "QmlObjectListModel.h" + +#include "Geometry/WimaArea.h" +#include "Geometry/WimaMeasurementArea.h" +#include "Geometry/WimaServiceArea.h" +#include "Geometry/WimaCorridor.h" +#include "Geometry/WimaMeasurementAreaData.h" +#include "Geometry/WimaCorridorData.h" +#include "Geometry/WimaServiceAreaData.h" + +#include "WimaPlanData.h" + +#include "PlanMasterController.h" +#include "MissionController.h" +#include "SurveyComplexItem.h" +#include "SimpleMissionItem.h" +#include "MissionSettingsItem.h" +#include "JsonHelper.h" +#include "QGCApplication.h" +#include "SettingsFact.h" +#include "WimaSettings.h" +#include "SettingsManager.h" + +#include "snake.h" +#include "Snake/SnakeWorker.h" +#include "Snake/GeoPolygonArray.h" +#include "Snake/PolygonArray.h" +#include "Geometry/GeoPoint3D.h" +#include "Snake/QNemoProgress.h" +#include "Snake/QNemoHeartbeat.h" + +#include "ros_bridge/include/ROSBridge.h" + +#include "WaypointManager/DefaultManager.h" +#include "WaypointManager/RTLManager.h" + +#include + +#define CHECK_BATTERY_INTERVAL 1000 // ms +#define SMART_RTL_MAX_ATTEMPTS 3 // times +#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms +#define EVENT_TIMER_INTERVAL 50 // ms +#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms + + +using namespace snake; + +typedef std::unique_ptr JsonDocUPtr; + +class WimaController : public QObject +{ + Q_OBJECT + + enum FileType {WimaFile, PlanFile}; + + typedef QScopedPointer ROSBridgePtr; + +public: + + + WimaController(QObject *parent = nullptr); + + // Controllers. + Q_PROPERTY(PlanMasterController* masterController + READ masterController + WRITE setMasterController + NOTIFY masterControllerChanged + ) + Q_PROPERTY(MissionController* missionController + READ missionController + WRITE setMissionController + NOTIFY missionControllerChanged + ) + // Wima Data. + Q_PROPERTY(QmlObjectListModel* visualItems + READ visualItems + NOTIFY visualItemsChanged + ) + Q_PROPERTY(QmlObjectListModel* missionItems + READ missionItems + NOTIFY missionItemsChanged + ) + Q_PROPERTY(QmlObjectListModel* currentMissionItems + READ currentMissionItems + NOTIFY currentMissionItemsChanged + ) + Q_PROPERTY(QVariantList waypointPath + READ waypointPath + NOTIFY waypointPathChanged + ) + Q_PROPERTY(QVariantList currentWaypointPath + READ currentWaypointPath + NOTIFY currentWaypointPathChanged + ) + Q_PROPERTY(Fact* enableWimaController + READ enableWimaController + CONSTANT) + // Waypoint navigaton. + Q_PROPERTY(Fact* overlapWaypoints + READ overlapWaypoints + CONSTANT + ) + Q_PROPERTY(Fact* maxWaypointsPerPhase + READ maxWaypointsPerPhase + CONSTANT + ) + Q_PROPERTY(Fact* startWaypointIndex + READ startWaypointIndex + CONSTANT + ) + Q_PROPERTY(Fact* showAllMissionItems + READ showAllMissionItems + CONSTANT + ) + Q_PROPERTY(Fact* showCurrentMissionItems + READ showCurrentMissionItems + CONSTANT + ) + // Waypoint settings. + Q_PROPERTY(Fact* flightSpeed + READ flightSpeed + CONSTANT + ) + Q_PROPERTY(Fact* altitude + READ altitude + CONSTANT + ) + Q_PROPERTY(Fact* arrivalReturnSpeed + READ arrivalReturnSpeed + CONSTANT + ) + // Waypoint statistics. + Q_PROPERTY(double phaseDistance + READ phaseDistance + NOTIFY phaseDistanceChanged + ) + Q_PROPERTY(double phaseDuration + READ phaseDuration + NOTIFY phaseDurationChanged + ) + + // Snake + Q_PROPERTY(Fact* enableSnake + READ enableSnake + CONSTANT + ) + Q_PROPERTY(int nemoStatus + READ nemoStatus + NOTIFY nemoStatusChanged + ) + Q_PROPERTY(QString nemoStatusString + READ nemoStatusString + NOTIFY nemoStatusStringChanged + ) + Q_PROPERTY(bool snakeCalcInProgress + READ snakeCalcInProgress + NOTIFY snakeCalcInProgressChanged + ) + Q_PROPERTY(Fact* snakeTileWidth + READ snakeTileWidth + CONSTANT + ) + Q_PROPERTY(Fact* snakeTileHeight + READ snakeTileHeight + CONSTANT + ) + Q_PROPERTY(Fact* snakeMinTileArea + READ snakeMinTileArea + CONSTANT + ) + Q_PROPERTY(Fact* snakeLineDistance + READ snakeLineDistance + CONSTANT + ) + Q_PROPERTY(Fact* snakeMinTransectLength + READ snakeMinTransectLength + CONSTANT + ) + Q_PROPERTY(QmlObjectListModel* snakeTiles + READ snakeTiles + NOTIFY snakeTilesChanged + ) + Q_PROPERTY(QVariantList snakeTileCenterPoints + READ snakeTileCenterPoints + NOTIFY snakeTileCenterPointsChanged + ) + Q_PROPERTY(QVector nemoProgress + READ nemoProgress + NOTIFY nemoProgressChanged + ) + + + + // Property accessors + // Controllers. + PlanMasterController* masterController (void); + MissionController* missionController (void); + // Wima Data + QmlObjectListModel* visualItems (void); + QGCMapPolygon joinedArea (void) const; + // Waypoints. + QmlObjectListModel* missionItems (void); + QmlObjectListModel* currentMissionItems (void); + QVariantList waypointPath (void); + QVariantList currentWaypointPath (void); + // Settings facts. + Fact* enableWimaController (void); + Fact* overlapWaypoints (void); + Fact* maxWaypointsPerPhase (void); + Fact* startWaypointIndex (void); + Fact* showAllMissionItems (void); + Fact* showCurrentMissionItems(void); + Fact* flightSpeed (void); + Fact* arrivalReturnSpeed (void); + Fact* altitude (void); + // Snake settings facts. + Fact* enableSnake (void) { return &_enableSnake; } + Fact* snakeTileWidth (void) { return &_snakeTileWidth;} + Fact* snakeTileHeight (void) { return &_snakeTileHeight;} + Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;} + Fact* snakeLineDistance (void) { return &_snakeLineDistance;} + Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;} + // Snake data. + QmlObjectListModel* snakeTiles (void) { return _snakeTiles.QmlObjectListModel();} + QVariantList snakeTileCenterPoints (void) { return _snakeTileCenterPoints;} + QVector nemoProgress (void); + int nemoStatus (void) const; + QString nemoStatusString (void) const; + bool snakeCalcInProgress (void) const; + + // Smart RTL. + bool uploadOverrideRequired (void) const; + bool vehicleHasLowBattery (void) const; + // Waypoint statistics. + double phaseDistance (void) const; + double phaseDuration (void) const; + + + // Property setters + void setMasterController (PlanMasterController* masterController); + void setMissionController (MissionController* missionController); + bool setWimaPlanData (const WimaPlanData &planData); + + // Member Methodes + Q_INVOKABLE WimaController *thisPointer(); + // Waypoint navigation. + Q_INVOKABLE void nextPhase(); + Q_INVOKABLE void previousPhase(); + Q_INVOKABLE void resetPhase(); + // Smart RTL. + Q_INVOKABLE void requestSmartRTL(); + Q_INVOKABLE void initSmartRTL(); + Q_INVOKABLE void executeSmartRTL(); + // Other. + Q_INVOKABLE void removeVehicleTrajectoryHistory(); + Q_INVOKABLE bool upload(); + Q_INVOKABLE bool forceUpload(); + Q_INVOKABLE void removeFromVehicle(); + + + // static Members + static const char* areaItemsName; + static const char* missionItemsName; + static const char* settingsGroup; + static const char* endWaypointIndexName; + static const char* enableWimaControllerName; + static const char* overlapWaypointsName; + static const char* maxWaypointsPerPhaseName; + static const char* startWaypointIndexName; + static const char* showAllMissionItemsName; + static const char* showCurrentMissionItemsName; + static const char* flightSpeedName; + static const char* arrivalReturnSpeedName; + static const char* altitudeName; + static const char* snakeTileWidthName; + static const char* snakeTileHeightName; + static const char* snakeMinTileAreaName; + static const char* snakeLineDistanceName; + static const char* snakeMinTransectLengthName; + +signals: + // Controllers. + void masterControllerChanged (void); + void missionControllerChanged (void); + // Wima data. + void visualItemsChanged (void); + // Waypoints. + void missionItemsChanged (void); + void currentMissionItemsChanged (void); + void waypointPathChanged (void); + void currentWaypointPathChanged (void); + // Smart RTL. + void smartRTLRequestConfirm (void); + void smartRTLPathConfirm (void); + // Upload. + void forceUploadConfirm (void); + // Waypoint statistics. + void phaseDistanceChanged (void); + void phaseDurationChanged (void); + // Snake. + void snakeConnectionStatusChanged (void); + void snakeCalcInProgressChanged (void); + void snakeTilesChanged (void); + void snakeTileCenterPointsChanged (void); + void nemoProgressChanged (void); + void nemoStatusChanged (void); + void nemoStatusStringChanged (void); + +private slots: + + // Waypoint navigation / helper. + bool _calcNextPhase (void); + void _recalcCurrentPhase (void); + bool _calcShortestPath (const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path); + // Slicing parameters + bool _setStartIndex (void); + void _updateOverlap (void); + void _updateMaxWaypoints (void); + // Waypoint settings. + void _updateflightSpeed (void); + void _updateArrivalReturnSpeed (void); + void _updateAltitude (void); + // Waypoint Statistics. + void _setPhaseDistance (double distance); + void _setPhaseDuration (double duration); + // SMART RTL + void _checkBatteryLevel (void); + bool _checkSmartRTLPreCondition (QString &errorString); + void _initSmartRTL (); + void _smartRTLCleanUp (bool flying); + // Snake. + void _setSnakeCalcInProgress (bool inProgress); + void _updateSnakeData (); + void _initStartSnakeWorker (); + void _switchSnakeManager (QVariant variant); + // Periodic tasks. + void _eventTimerHandler (void); + // Waypoint manager handling. + void _switchWaypointManager(WaypointManager::ManagerBase &manager); + +private: + using StatusMap = std::map; + + // Controllers. + PlanMasterController *_masterController; + MissionController *_missionController; + + // Wima Data. + QmlObjectListModel _areas; // contains all visible areas + WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor + WimaMeasurementAreaData _measurementArea; // measurement area + WimaServiceAreaData _serviceArea; // area for supplying + WimaCorridorData _corridor; // corridor connecting opArea and serArea + bool _localPlanDataValid; + + // Waypoint Managers. + WaypointManager::AreaInterface _areaInterface; + WaypointManager::Settings _managerSettings; + WaypointManager::DefaultManager _defaultManager; + WaypointManager::DefaultManager _snakeManager; + WaypointManager::RTLManager _rtlManager; + WaypointManager::ManagerBase *_currentManager; + using ManagerList = QList; + ManagerList _managerList; + + // Settings Facts. + QMap _metaDataMap; + SettingsFact _enableWimaController; // enables or disables the wimaControler + SettingsFact _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases + SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints per phase + SettingsFact _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item + // defining the first element of the next phase + SettingsFact _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not. + SettingsFact _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not. + SettingsFact _flightSpeed; // mission flight speed + SettingsFact _arrivalReturnSpeed; // arrival and return path speed + SettingsFact _altitude; // mission altitude + SettingsFact _enableSnake; // Enable Snake (see snake.h) + SettingsFact _snakeTileWidth; + SettingsFact _snakeTileHeight; + SettingsFact _snakeMinTileArea; + SettingsFact _snakeLineDistance; + SettingsFact _snakeMinTransectLength; + + // Smart RTL. + QTimer _smartRTLTimer; + bool _lowBatteryHandlingTriggered; + + // Waypoint statistics. + double _measurementPathLength; // the lenght of the phase in meters +// double _phaseDistance; // the lenth in meters of the current phase +// double _phaseDuration; // the phase duration in seconds + + // Snake + bool _snakeCalcInProgress; + SnakeDataManager _snakeWorker; + GeoPoint _snakeOrigin; + GeoPolygonArray _snakeTiles; // tiles + PolygonArray _snakeTilesLocal; // tiles local coordinate system + QVariantList _snakeTileCenterPoints; + QNemoProgress _nemoProgress; // measurement progress + QNemoHeartbeat _nemoHeartbeat; // measurement progress + int _fallbackStatus; + ROSBridgePtr _pRosBridge; + bool _bridgeSetupDone; + static StatusMap _nemoStatusMap; + + // Periodic tasks. + QTimer _eventTimer; + +}; + + diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index 039e60f5ec76221a834fd99bfabbdbb2507d7a9a..83e6febe0fe31401e6af2b1ccd3d91c197cd0a5c 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -10,101 +10,98 @@ #include "rapidjson/include/rapidjson/document.h" #include "rapidjson/include/rapidjson/writer.h" #include "rapidjson/include/rapidjson/stringbuffer.h" -#include "rapidjson/include/rapidjson/ostreamwrapper.h" #include #include #include #include -#include +#include using WsClient = SimpleWeb::SocketClient; using InMessage = std::function, std::shared_ptr)>; class RosbridgeWsClient { - std::string server_port_path; - std::unordered_map> client_map; - - // Starts the client. - void start(const std::string& client_name, std::shared_ptr client, const std::string& message) - { - if (!client->on_open) - { -#ifdef ROS_BRIDGE_CLIENT_DEBUG - client->on_open = [client_name, message](std::shared_ptr connection) { + std::string server_port_path; + std::unordered_map> client_map; + std::mutex map_mutex; + + void start(const std::string& client_name, std::shared_ptr client, const std::string& message) + { + if (!client->on_open) + { +#ifdef DEBUG + client->on_open = [client_name, message](std::shared_ptr connection) { #else - client->on_open = [message](std::shared_ptr connection) { + client->on_open = [message](std::shared_ptr connection) { #endif -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::cout << client_name << ": Opened connection" << std::endl; - std::cout << client_name << ": Sending message: " << message << std::endl; +#ifdef DEBUG + std::cout << client_name << ": Opened connection" << std::endl; + std::cout << client_name << ": Sending message: " << message << std::endl; #endif - connection->send(message); - }; - } - -#ifdef ROS_BRIDGE_CLIENT_DEBUG - if (!client->on_message) - { - client->on_message = [client_name](std::shared_ptr /*connection*/, std::shared_ptr in_message) { - std::cout << client_name << ": Message received: " << in_message->string() << std::endl; - }; - } - - if (!client->on_close) - { - client->on_close = [client_name](std::shared_ptr /*connection*/, int status, const std::string & /*reason*/) { - std::cout << client_name << ": Closed connection with status code " << status << std::endl; - }; - } - - if (!client->on_error) - { - // See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings - client->on_error = [client_name](std::shared_ptr /*connection*/, const SimpleWeb::error_code &ec) { - std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl; - }; - } + connection->send(message); + }; + } + +#ifdef DEBUG + if (!client->on_message) + { + client->on_message = [client_name](std::shared_ptr /*connection*/, std::shared_ptr in_message) { + std::cout << client_name << ": Message received: " << in_message->string() << std::endl; + }; + } + + if (!client->on_close) + { + client->on_close = [client_name](std::shared_ptr /*connection*/, int status, const std::string & /*reason*/) { + std::cout << client_name << ": Closed connection with status code " << status << std::endl; + }; + } + + if (!client->on_error) + { + // See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings + client->on_error = [client_name](std::shared_ptr /*connection*/, const SimpleWeb::error_code &ec) { + std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl; + }; + } #endif -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::thread client_thread([client_name, client]() { +#ifdef DEBUG + std::thread client_thread([client_name, client]() { #else - std::thread client_thread([client]() { + std::thread client_thread([client]() { #endif - client->start(); + client->start(); -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::cout << client_name << ": Terminated" << std::endl; +#ifdef DEBUG + std::cout << client_name << ": Terminated" << std::endl; #endif - client->on_open = NULL; - client->on_message = NULL; - client->on_close = NULL; - client->on_error = NULL; - }); - - client_thread.detach(); + client->on_open = NULL; + client->on_message = NULL; + client->on_close = NULL; + client->on_error = NULL; + }); - // This is to make sure that the thread got fully launched before we do anything to it (e.g. remove) - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } + client_thread.detach(); + } public: - RosbridgeWsClient(const std::string& server_port_path) - { - this->server_port_path = server_port_path; - } - - ~RosbridgeWsClient() - { - for (auto& client : client_map) - { - client.second->stop(); - client.second.reset(); - } - } + RosbridgeWsClient(const std::string& server_port_path) + { + this->server_port_path = server_port_path; + } + + ~RosbridgeWsClient() + { + std::lock_guard lk(map_mutex); // neccessary? + for (auto& client : client_map) + { + client.second->stop(); + client.second.reset(); + } + } // The execution can take up to 100 ms! bool connected(){ @@ -133,267 +130,386 @@ public: return status == std::future_status::ready; } - // Adds a client to the client_map. - void addClient(const std::string& client_name) - { - std::unordered_map>::iterator it = client_map.find(client_name); - if (it == client_map.end()) - { - client_map[client_name] = std::make_shared(server_port_path); - } -#ifdef ROS_BRIDGE_CLIENT_DEBUG - else - { - std::cerr << client_name << " has already been created" << std::endl; - } + // Adds a client to the client_map. + void addClient(const std::string& client_name) + { + std::lock_guard lk(map_mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it == client_map.end()) + { + client_map[client_name] = std::make_shared(server_port_path); + } +#ifdef DEBUG + else + { + std::cerr << client_name << " has already been created" << std::endl; + } #endif - } - - std::shared_ptr getClient(const std::string& client_name) - { - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - return it->second; - } - return NULL; - } - - void stopClient(const std::string& client_name) - { - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - it->second->stop(); - it->second->on_open = NULL; - it->second->on_message = NULL; - it->second->on_close = NULL; - it->second->on_error = NULL; -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::cout << client_name << " has been stopped" << std::endl; + } + + std::shared_ptr getClient(const std::string& client_name) + { + std::lock_guard lk(map_mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + return it->second; + } + return NULL; + } + + void stopClient(const std::string& client_name) + { + std::lock_guard lk(map_mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + it->second->stop(); + it->second->on_open = NULL; + it->second->on_message = NULL; + it->second->on_close = NULL; + it->second->on_error = NULL; +#ifdef DEBUG + std::cout << client_name << " has been stopped" << std::endl; #endif - } -#ifdef ROS_BRIDGE_CLIENT_DEBUG - else - { - std::cerr << client_name << " has not been created" << std::endl; - } + } +#ifdef DEBUG + else + { + std::cerr << client_name << " has not been created" << std::endl; + } #endif - } - - void removeClient(const std::string& client_name) - { - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - it->second->stop(); - it->second.reset(); - client_map.erase(it); -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::cout << client_name << " has been removed" << std::endl; + } + + void removeClient(const std::string& client_name) + { + std::lock_guard lk(map_mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + // Stop the client asynchronously in 100 ms. + // This is to ensure, that all threads involving the client have been launched. + std::thread t([](std::shared_ptr client){ + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + client->stop(); + client.reset(); + }, + it->second /*lambda param*/ ); + client_map.erase(it); + t.detach(); +#ifdef DEBUG + std::cout << client_name << " has been removed" << std::endl; #endif - } -#ifdef ROS_BRIDGE_CLIENT_DEBUG - else - { - std::cerr << client_name << " has not been created" << std::endl; - } + } +#ifdef DEBUG + else + { + std::cerr << client_name << " has not been created" << std::endl; + } #endif - } - - // Gets the client from client_map and starts it. Advertising is essentially sending a message. - // One client per topic! - void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "") - { - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\""; - - if (id.compare("") != 0) - { - message += ", \"id\":\"" + id + "\""; - } - message = "{" + message + "}"; - - start(client_name, it->second, message); - } -#ifdef ROS_BRIDGE_CLIENT_DEBUG - else - { - std::cerr << client_name << "has not been created" << std::endl; - } + } + + // Gets the client from client_map and starts it. Advertising is essentially sending a message. + // One client per topic! + void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "") + { + std::lock_guard lk(map_mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\""; + + if (id.compare("") != 0) + { + message += ", \"id\":\"" + id + "\""; + } + message = "{" + message + "}"; + + start(client_name, it->second, message); + } +#ifdef DEBUG + else + { + std::cerr << client_name << "has not been created" << std::endl; + } #endif - } + } - void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "") - { - rapidjson::StringBuffer strbuf; - rapidjson::Writer writer(strbuf); - msg.Accept(writer); + void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "") + { + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + msg.Accept(writer); - std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString(); + std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString(); - if (id.compare("") != 0) - { - message += ", \"id\":\"" + id + "\""; - } - message = "{" + message + "}"; + if (id.compare("") != 0) + { + message += ", \"id\":\"" + id + "\""; + } + message = "{" + message + "}"; - std::shared_ptr publish_client = std::make_shared(server_port_path); + std::shared_ptr publish_client = std::make_shared(server_port_path); - publish_client->on_open = [message](std::shared_ptr connection) { -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::cout << "publish_client: Opened connection" << std::endl; - std::cout << "publish_client: Sending message: " << message << std::endl; + publish_client->on_open = [message](std::shared_ptr connection) { +#ifdef DEBUG + std::cout << "publish_client: Opened connection" << std::endl; + std::cout << "publish_client: Sending message: " << message << std::endl; #endif - connection->send(message); + connection->send(message); - // TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away - connection->send_close(1000); - }; - - start("publish_client", publish_client, message); - } + // TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away + connection->send_close(1000); + }; - void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "") - { - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - std::string message = "\"op\":\"subscribe\", \"topic\":\"" + topic + "\""; - - if (id.compare("") != 0) - { - message += ", \"id\":\"" + id + "\""; - } - if (type.compare("") != 0) - { - message += ", \"type\":\"" + type + "\""; - } - if (throttle_rate > -1) - { - message += ", \"throttle_rate\":" + std::to_string(throttle_rate); - } - if (queue_length > -1) - { - message += ", \"queue_length\":" + std::to_string(queue_length); - } - if (fragment_size > -1) - { - message += ", \"fragment_size\":" + std::to_string(fragment_size); - } - if (compression.compare("none") == 0 || compression.compare("png") == 0) - { - message += ", \"compression\":\"" + compression + "\""; - } - message = "{" + message + "}"; - - it->second->on_message = callback; - start(client_name, it->second, message); - } -#ifdef ROS_BRIDGE_CLIENT_DEBUG - else - { - std::cerr << client_name << "has not been created" << std::endl; - } + start("publish_client", publish_client, message); + } + + void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "") + { + std::lock_guard lk(map_mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + std::string message = "\"op\":\"subscribe\", \"topic\":\"" + topic + "\""; + + if (id.compare("") != 0) + { + message += ", \"id\":\"" + id + "\""; + } + if (type.compare("") != 0) + { + message += ", \"type\":\"" + type + "\""; + } + if (throttle_rate > -1) + { + message += ", \"throttle_rate\":" + std::to_string(throttle_rate); + } + if (queue_length > -1) + { + message += ", \"queue_length\":" + std::to_string(queue_length); + } + if (fragment_size > -1) + { + message += ", \"fragment_size\":" + std::to_string(fragment_size); + } + if (compression.compare("none") == 0 || compression.compare("png") == 0) + { + message += ", \"compression\":\"" + compression + "\""; + } + message = "{" + message + "}"; + + it->second->on_message = callback; + start(client_name, it->second, message); + } +#ifdef DEBUG + else + { + std::cerr << client_name << "has not been created" << std::endl; + } #endif - } - - void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback) - { - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - std::string message = "{\"op\":\"advertise_service\", \"service\":\"" + service + "\", \"type\":\"" + type + "\"}"; - - it->second->on_message = callback; - start(client_name, it->second, message); - } -#ifdef ROS_BRIDGE_CLIENT_DEBUG - else - { - std::cerr << client_name << "has not been created" << std::endl; - } + } + + void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback) + { + std::lock_guard lk(map_mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + std::string message = "{\"op\":\"advertise_service\", \"service\":\"" + service + "\", \"type\":\"" + type + "\"}"; + + it->second->on_message = callback; + start(client_name, it->second, message); + } +#ifdef DEBUG + else + { + std::cerr << client_name << "has not been created" << std::endl; + } #endif - } + } - void serviceResponse(const std::string& service, const std::string& id, bool result, const rapidjson::Document& values = {}) - { - std::string message = "\"op\":\"service_response\", \"service\":\"" + service + "\", \"result\":" + ((result)? "true" : "false"); + void serviceResponse(const std::string& service, const std::string& id, bool result, const rapidjson::Document& values = {}) + { + std::string message = "\"op\":\"service_response\", \"service\":\"" + service + "\", \"result\":" + ((result)? "true" : "false"); - // Rosbridge somehow does not allow service_response to be sent without id and values - // , so we cannot omit them even though the documentation says they are optional. - message += ", \"id\":\"" + id + "\""; + // Rosbridge somehow does not allow service_response to be sent without id and values + // , so we cannot omit them even though the documentation says they are optional. + message += ", \"id\":\"" + id + "\""; - // Convert JSON document to string - rapidjson::StringBuffer strbuf; - rapidjson::Writer writer(strbuf); - values.Accept(writer); + // Convert JSON document to string + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + values.Accept(writer); - message += ", \"values\":" + std::string(strbuf.GetString()); - message = "{" + message + "}"; + message += ", \"values\":" + std::string(strbuf.GetString()); + message = "{" + message + "}"; - std::shared_ptr service_response_client = std::make_shared(server_port_path); + std::shared_ptr service_response_client = std::make_shared(server_port_path); - service_response_client->on_open = [message](std::shared_ptr connection) { -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::cout << "service_response_client: Opened connection" << std::endl; - std::cout << "service_response_client: Sending message: " << message << std::endl; + service_response_client->on_open = [message](std::shared_ptr connection) { +#ifdef DEBUG + std::cout << "service_response_client: Opened connection" << std::endl; + std::cout << "service_response_client: Sending message: " << message << std::endl; #endif - connection->send(message); - - connection->send_close(1000); - }; + connection->send(message); - start("service_response_client", service_response_client, message); - } + connection->send_close(1000); + }; - void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "") - { - std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\""; - - if (!args.IsNull()) - { - rapidjson::StringBuffer strbuf; - rapidjson::Writer writer(strbuf); - args.Accept(writer); - - message += ", \"args\":" + std::string(strbuf.GetString()); - } - if (id.compare("") != 0) - { - message += ", \"id\":\"" + id + "\""; - } - if (fragment_size > -1) - { - message += ", \"fragment_size\":" + std::to_string(fragment_size); - } - if (compression.compare("none") == 0 || compression.compare("png") == 0) - { - message += ", \"compression\":\"" + compression + "\""; - } - message = "{" + message + "}"; - - std::shared_ptr call_service_client = std::make_shared(server_port_path); - - if (callback) - { - call_service_client->on_message = callback; - } - else - { - call_service_client->on_message = [](std::shared_ptr connection, std::shared_ptr in_message) { -#ifdef ROS_BRIDGE_CLIENT_DEBUG - std::cout << "call_service_client: Message received: " << in_message->string() << std::endl; - std::cout << "call_service_client: Sending close connection" << std::endl; + start("service_response_client", service_response_client, message); + } + + void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "") + { + std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\""; + + if (!args.IsNull()) + { + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + args.Accept(writer); + + message += ", \"args\":" + std::string(strbuf.GetString()); + } + if (id.compare("") != 0) + { + message += ", \"id\":\"" + id + "\""; + } + if (fragment_size > -1) + { + message += ", \"fragment_size\":" + std::to_string(fragment_size); + } + if (compression.compare("none") == 0 || compression.compare("png") == 0) + { + message += ", \"compression\":\"" + compression + "\""; + } + message = "{" + message + "}"; + + std::shared_ptr call_service_client = std::make_shared(server_port_path); + + if (callback) + { + call_service_client->on_message = callback; + } + else + { + call_service_client->on_message = [](std::shared_ptr connection, std::shared_ptr in_message) { +#ifdef DEBUG + std::cout << "call_service_client: Message received: " << in_message->string() << std::endl; + std::cout << "call_service_client: Sending close connection" << std::endl; #endif - connection->send_close(1000); - }; - } - - start("call_service_client", call_service_client, message); - } + connection->send_close(1000); + }; + } + + start("call_service_client", call_service_client, message); + } + + //! + //! \brief Checks if the service \p service is available. + //! \param service Service name. + //! \return Returns true if the service is available, false either. + //! \note Don't call this function to frequently. Use \fn waitForService() instead. + //! + bool serviceAvailable(const std::string& service) + { + const rapidjson::Document args = {}; + const std::string& id = ""; + const int fragment_size = -1; + const std::string& compression = ""; + std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\""; + std::string client_name("wait_for_service_client"); + + if (!args.IsNull()) + { + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + args.Accept(writer); + + message += ", \"args\":" + std::string(strbuf.GetString()); + } + if (id.compare("") != 0) + { + message += ", \"id\":\"" + id + "\""; + } + if (fragment_size > -1) + { + message += ", \"fragment_size\":" + std::to_string(fragment_size); + } + if (compression.compare("none") == 0 || compression.compare("png") == 0) + { + message += ", \"compression\":\"" + compression + "\""; + } + message = "{" + message + "}"; + + std::shared_ptr wait_for_service_client = std::make_shared(server_port_path); + std::shared_ptr> p(std::make_shared>()); + auto future = p->get_future(); +#ifdef DEBUG + wait_for_service_client->on_message = [p, service, client_name]( +#else + wait_for_service_client->on_message = [p]( +#endif + std::shared_ptr connection, + std::shared_ptr in_message){ +#ifdef DEBUG +#endif + rapidjson::Document doc; + doc.Parse(in_message->string().c_str()); + if ( !doc.HasParseError() + && doc.HasMember("result") + && doc["result"].IsBool() + && doc["result"] == true ) + { +#ifdef DEBUG + std::cout << client_name << ": " + << "service " << service + << " available." << std::endl; + std::cout << client_name << ": " + << "message: " << in_message->string() + << std::endl; +#endif + p->set_value(true); + } else { + p->set_value(false); + } + connection->send_close(1000); + }; + start(client_name, wait_for_service_client, message); + return future.get(); + } + + //! + //! \brief Waits until the service with the name \p service is available. + //! \param service Service name. + //! @note This function will block as long as the service is not available. + //! + void waitForService(const std::string& service) + { + + +#ifdef DEBUG + auto s = std::chrono::high_resolution_clock::now(); + long counter = 0; +#endif + while(true) + { +#ifdef DEBUG + ++counter; +#endif + if (serviceAvailable(service)){ + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); // Avoid excessive polling. + }; +#ifdef DEBUG + auto e = std::chrono::high_resolution_clock::now(); + std::cout << "waitForService(): time: " + << std::chrono::duration_cast(e-s).count() + << " ms." << std::endl; + std::cout << "waitForService(): clients launched: " + << counter << std::endl; +#endif + } }; - - diff --git a/src/comm/ros_bridge/include/Server.cpp b/src/comm/ros_bridge/include/Server.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a4907064da92744543fbb6670e4bcd633d85d1e0 --- /dev/null +++ b/src/comm/ros_bridge/include/Server.cpp @@ -0,0 +1,6 @@ +#include "Server.h" + +Server::Server() +{ + +} diff --git a/src/comm/ros_bridge/include/Server.h b/src/comm/ros_bridge/include/Server.h new file mode 100644 index 0000000000000000000000000000000000000000..5cbd2374bc68ba117dcc76f3ac12f8810632cbc9 --- /dev/null +++ b/src/comm/ros_bridge/include/Server.h @@ -0,0 +1,11 @@ +#ifndef SERVER_H +#define SERVER_H + + +class Server +{ +public: + Server(); +}; + +#endif // SERVER_H diff --git a/src/comm/ros_bridge/include/ThreadSafeQueue.h b/src/comm/ros_bridge/include/ThreadSafeQueue.h deleted file mode 100644 index 03d653eeeacf2f6be8bb6a8c84c830054d9daaf5..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/ThreadSafeQueue.h +++ /dev/null @@ -1,79 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace ROSBridge { - -template -class ThreadSafeQueue -{ -public: - ThreadSafeQueue(); - ~ThreadSafeQueue(); - - T pop_front(); - - void push_back(const T& item); - void push_back(T&& item); - - int size(); - bool empty(); - -private: - std::deque _queue; - std::mutex _mutex; - std::condition_variable _cond; -}; - -template -ThreadSafeQueue::ThreadSafeQueue(){} - -template -ThreadSafeQueue::~ThreadSafeQueue(){} - -template -T ThreadSafeQueue::pop_front() -{ - std::unique_lock mlock(_mutex); - while (_queue.empty()) - { - _cond.wait(mlock); - } - T t = std::move(_queue.front()); - _queue.pop_front(); - return t; -} - -template -void ThreadSafeQueue::push_back(const T& item) -{ - std::unique_lock mlock(_mutex); - _queue.push_back(item); - mlock.unlock(); // unlock before notificiation to minimize mutex con - _cond.notify_one(); // notify one waiting thread - -} - -template -void ThreadSafeQueue::push_back(T&& item) -{ - std::unique_lock mlock(_mutex); - _queue.push_back(std::move(item)); - mlock.unlock(); // unlock before notificiation to minimize mutex con - _cond.notify_one(); // notify one waiting thread - -} - -template -int ThreadSafeQueue::size() -{ - std::unique_lock mlock(_mutex); - int size = _queue.size(); - mlock.unlock(); - return size; -} - -} // namespace -