From 591ba07efbe9d9ad3399faa101614fa91536dfc0 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 24 Aug 2020 15:41:58 +0200 Subject: [PATCH] 123 --- qgroundcontrol.pro | 3 +- src/Wima/Geometry/WimaPolygonArray.h | 7 +- src/Wima/WimaController.cc | 151 +-- src/Wima/WimaController.h | 3 +- src/Wima/WimaController_new.cc | 924 ------------------ src/Wima/WimaController_new.h | 421 -------- .../ros_bridge/include/RosBridgeClient.cpp | 162 +-- src/comm/ros_bridge/include/com_private.cpp | 8 - .../messages/geographic_msgs/geopoint.cpp | 2 +- .../messages/geographic_msgs/geopoint.h | 3 - .../include/messages/geometry_msgs/point32.h | 3 - .../include/messages/geometry_msgs/polygon.h | 3 - .../messages/geometry_msgs/polygon_stamped.h | 3 - .../jsk_recognition_msgs/polygon_array.h | 4 - .../include/messages/nemo_msgs/heartbeat.h | 3 - .../include/messages/nemo_msgs/progress.h | 3 - .../include/messages/std_msgs/header.h | 3 - .../include/messages/std_msgs/time.h | 3 - src/comm/ros_bridge/include/ros_bridge.h | 5 +- .../ros_bridge/include/topic_publisher.cpp | 93 +- src/comm/ros_bridge/include/topic_publisher.h | 3 + src/comm/ros_bridge/src/ros_bridge.cpp | 10 + 22 files changed, 264 insertions(+), 1556 deletions(-) delete mode 100644 src/Wima/WimaController_new.cc delete mode 100644 src/Wima/WimaController_new.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 9697d4820..f3f0ceabe 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -28,11 +28,12 @@ QGCROOT = $$PWD DebugBuild { DESTDIR = $${OUT_PWD}/debug DEFINES += DEBUG + DEFINES += ROS_BRIDGE_DEBUG #DEFINES += ROS_BRIDGE_CLIENT_DEBUG } else { DESTDIR = $${OUT_PWD}/release - DEFINES += DEBUG + DEFINES += ROS_BRIDGE_DEBUG DEFINES += NDEBUG } diff --git a/src/Wima/Geometry/WimaPolygonArray.h b/src/Wima/Geometry/WimaPolygonArray.h index c489ab15f..6d662ad00 100644 --- a/src/Wima/Geometry/WimaPolygonArray.h +++ b/src/Wima/Geometry/WimaPolygonArray.h @@ -13,6 +13,9 @@ public: WimaPolygonArray(const WimaPolygonArray &other) : _polygons(other._polygons), _dirty(true) {} + ~WimaPolygonArray(){ + _objs.clearAndDeleteContents(); + } class QmlObjectListModel *QmlObjectListModel(){ if (_dirty) @@ -34,9 +37,9 @@ public: private: void _updateObjects(void){ - _objs.clear(); + _objs.clearAndDeleteContents(); for (long i=0; i<_polygons.size(); ++i){ - _objs.append(&_polygons[i]); + _objs.append(new PolygonType(_polygons[i])); } } diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 5ddcf2669..ec0f6b0f6 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -89,6 +89,7 @@ WimaController::WimaController(QObject *parent) , _batteryLevelTicker (EVENT_TIMER_INTERVAL, 1000 /*ms*/) , _snakeTicker (EVENT_TIMER_INTERVAL, 200 /*ms*/) , _nemoTimeoutTicker (EVENT_TIMER_INTERVAL, 5000 /*ms*/) + , _topicServiceSetupDone (false) { // ROS Bridge. @@ -551,23 +552,29 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) emit snakeTileCenterPointsChanged(); if ( _enableSnake.rawValue().toBool() - && _pRosBridge->isRunning() - && _pRosBridge->connected() ){ - if ( _snakeTilesLocal.polygons().size() > 0 ){ - using namespace ros_bridge::messages; - // Publish snake origin. - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - Q_ASSERT(geographic_msgs::geo_point::toJson( - _snakeOrigin, *jOrigin, jOrigin->GetAllocator())); - _pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - // Publish snake tiles. - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson( - _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator())); - _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - } + && _pRosBridge->isRunning() + && _pRosBridge->connected() + && _topicServiceSetupDone + && _snakeTilesLocal.polygons().size() > 0 + ) + { + using namespace ros_bridge::messages; + // Publish snake origin. + JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); + bool ret = geographic_msgs::geo_point::toJson( + _snakeOrigin, *jOrigin, jOrigin->GetAllocator()); + Q_ASSERT(ret); + _pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + // Publish snake tiles. + JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); + ret = jsk_recognition_msgs::polygon_array::toJson( + _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()); + Q_ASSERT(ret); + _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); + } + _localPlanDataValid = true; return true; } @@ -758,21 +765,24 @@ void WimaController::_eventTimerHandler() } if ( _snakeTicker.ready() ) { - static bool setupDone = false; if ( _enableSnake.rawValue().toBool() ) { if ( !_pRosBridge->isRunning()) { _pRosBridge->start(); - } else if ( _pRosBridge->isRunning() && _pRosBridge->connected() && !setupDone) { + } else if ( _pRosBridge->isRunning() + && _pRosBridge->connected() + && !_topicServiceSetupDone) { + if ( _doTopicServiceSetup() ) + _topicServiceSetupDone = true; + } else if ( _pRosBridge->isRunning() + && !_pRosBridge->connected() + && _topicServiceSetupDone){ _pRosBridge->reset(); _pRosBridge->start(); - _setupTopicService(); - setupDone = true; - } else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){ - setupDone = false; + _topicServiceSetupDone = false; } } else if ( _pRosBridge->isRunning() ) { _pRosBridge->reset(); - setupDone = false; + _topicServiceSetupDone = false; } } } @@ -948,21 +958,35 @@ void WimaController::_switchSnakeManager(QVariant variant) } } -void WimaController::_setupTopicService() +bool WimaController::_doTopicServiceSetup() { using namespace ros_bridge::messages; - if ( _snakeTilesLocal.polygons().size() > 0){ - // Publish snake origin. - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - Q_ASSERT(geographic_msgs::geo_point::toJson( - _snakeOrigin, *jOrigin, jOrigin->GetAllocator())); - _pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - // Publish snake tiles. - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson( - _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator())); - _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - } + + 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){ @@ -971,24 +995,30 @@ void WimaController::_setupTopicService() 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 origin and tiles again, if valid. - if ( this->_snakeTilesLocal.polygons().size() > 0){ - // Publish snake origin. - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - Q_ASSERT(geographic_msgs::geo_point::toJson( - this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator())); - this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - // Publish snake tiles. - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson( - this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator())); - this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - } + 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; @@ -1004,31 +1034,38 @@ void WimaController::_setupTopicService() 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); - if ( this->_snakeTilesLocal.polygons().size() > 0){ - geographic_msgs::geo_point::toJson( - this->_snakeOrigin, jOrigin, pDoc->GetAllocator()); - } else { - geographic_msgs::geo_point::toJson( - ::GeoPoint3D(0,0,0), jOrigin, pDoc->GetAllocator()); - } + 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); - jsk_recognition_msgs::polygon_array::toJson( + 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; } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 24f53a796..936e76f4b 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -337,7 +337,7 @@ private slots: void _snakeStoreWorkerResults (); void _initStartSnakeWorker (); void _switchSnakeManager (QVariant variant); - void _setupTopicService (); + bool _doTopicServiceSetup(); // Periodic tasks. void _eventTimerHandler (void); // Waypoint manager handling. @@ -407,6 +407,7 @@ private: int _fallbackStatus; ROSBridgePtr _pRosBridge; static StatusMap _nemoStatusMap; + bool _topicServiceSetupDone; // Periodic tasks. QTimer _eventTimer; diff --git a/src/Wima/WimaController_new.cc b/src/Wima/WimaController_new.cc deleted file mode 100644 index 24bf234c5..000000000 --- a/src/Wima/WimaController_new.cc +++ /dev/null @@ -1,924 +0,0 @@ -#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 deleted file mode 100644 index c2e49dcfa..000000000 --- a/src/Wima/WimaController_new.h +++ /dev/null @@ -1,421 +0,0 @@ -#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.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp index 5b79aff28..b423b1abc 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.cpp +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -18,18 +18,18 @@ struct Task{ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr client, const std::__cxx11::string &message) { -#ifndef DEBUG +#ifndef ROS_BRIDGE_DEBUG (void)client_name; #endif if (!client->on_open) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG client->on_open = [client_name, message](std::shared_ptr connection) { #else client->on_open = [message](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; #endif @@ -37,7 +37,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar }; } -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG if (!client->on_message) { client->on_message = [client_name](std::shared_ptr /*connection*/, std::shared_ptr in_message) { @@ -61,21 +61,21 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar } #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::thread client_thread([client_name, client]() { #else std::thread client_thread([client]() { #endif client->start(); -#ifdef DEBUG +#ifdef ROS_BRIDGE_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; -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << " thread end" << std::endl; #endif }); @@ -122,7 +122,7 @@ void RosbridgeWsClient::run() // ==================================================================================== if ( std::chrono::high_resolution_clock::now() > poll_time_point) { poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << "Starting new poll." << std::endl; std::cout << "connected: " << this->isConnected->load() << std::endl; #endif @@ -133,14 +133,14 @@ void RosbridgeWsClient::run() return t.name == reset_status_task_name; }); if ( it == task_list.end() ){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << "Adding status_task" << std::endl; #endif // Check connection status. auto status_set = std::make_shared(false); auto status_client = std::make_shared(this->server_port_path); status_client->on_open = [status_set, this](std::shared_ptr) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << "status_client opened" << std::endl; #endif this->isConnected->store(true); @@ -190,16 +190,13 @@ void RosbridgeWsClient::run() }); if ( topics_it == task_list.end() ){ // Call /rosapi/topics service. -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << "Adding reset_topics_task" << std::endl; #endif auto topics_set = std::make_shared(false); this->callService("/rosapi/topics", [topics_set, this]( std::shared_ptr connection, std::shared_ptr in_message){ -#ifdef DEBUG - std::cout << "/rosapi/topics: " << in_message->string() << std::endl; -#endif std::unique_lock lk(this->mutex); this->available_topics = in_message->string(); lk.unlock(); @@ -241,16 +238,13 @@ void RosbridgeWsClient::run() }); if ( services_it == task_list.end() ){ // Call /rosapi/services service. -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << "Adding reset_services_task" << std::endl; #endif auto services_set = std::make_shared(false); this->callService("/rosapi/services", [this, services_set]( std::shared_ptr connection, std::shared_ptr in_message){ -#ifdef DEBUG - std::cout << "/rosapi/services: " << in_message->string() << std::endl; -#endif std::unique_lock lk(this->mutex); this->available_services = in_message->string(); lk.unlock(); @@ -295,26 +289,14 @@ void RosbridgeWsClient::run() // Process tasks. // ==================================================================================== for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){ -#ifdef DEBUG - std::cout << "processing task: " << task_it->name << std::endl; -#endif if ( !task_it->expired() ){ if ( task_it->ready() ){ -#ifdef DEBUG - std::cout << "executing task: " << task_it->name << std::endl; -#endif task_it->execute(); task_it = task_list.erase(task_it); } else { -#ifdef DEBUG - std::cout << "noting to do for task: " << task_it->name << std::endl; -#endif ++task_it; } } else { -#ifdef DEBUG - std::cout << "task expired: " << task_it->name << std::endl; -#endif task_it->clear_up(); task_it = task_list.erase(task_it); } @@ -328,7 +310,7 @@ void RosbridgeWsClient::run() task_it->clear_up(); } task_list.clear(); -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << "periodic thread end" << std::endl; #endif }); @@ -349,9 +331,14 @@ void RosbridgeWsClient::reset() unsubscribeAll(); unadvertiseAll(); unadvertiseAllServices(); - for (auto& client : client_map) - { - removeClient(client.first); + + std::unique_lock lk(mutex); + for (auto it = client_map.begin(); it != client_map.end(); ) { + std::string client_name = it->first; + lk.unlock(); + removeClient(client_name); + lk.lock(); + it = client_map.begin(); } } @@ -363,7 +350,7 @@ void RosbridgeWsClient::addClient(const std::string &client_name) { client_map[client_name] = std::make_shared(server_port_path); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG else { std::cerr << client_name << " has already been created" << std::endl; @@ -391,7 +378,7 @@ void RosbridgeWsClient::stopClient(const std::string &client_name) // Stop the client asynchronously in 100 ms. // This is to ensure, that all threads involving the client have been launched. std::shared_ptr client = it->second; -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::thread t([client, client_name](){ #else std::thread t([client](){ @@ -403,14 +390,13 @@ void RosbridgeWsClient::stopClient(const std::string &client_name) // client->on_message = NULL; // client->on_close = NULL; // client->on_error = NULL; -#ifdef DEBUG - std::cout << "removeClient thread: " << client_name << " reference count: " << client.use_count() << std::endl; +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << " has been removed" << std::endl; #endif }); t.detach(); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG else { std::cerr << client_name << " has not been created" << std::endl; @@ -428,7 +414,7 @@ void RosbridgeWsClient::removeClient(const std::string &client_name) { client_map.erase(it); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG else { std::cerr << client_name << " has not been created" << std::endl; @@ -448,15 +434,24 @@ std::string RosbridgeWsClient::getAdvertisedServices(){ } bool RosbridgeWsClient::topicAvailable(const std::string &topic){ -#ifdef DEBUG + std::lock_guard lk(mutex); +#ifdef ROS_BRIDGE_DEBUG std::cout << "checking if topic " << topic << " is available" << std::endl; + std::cout << "available topics: " << available_topics << std::endl; #endif size_t pos; { - std::lock_guard lk(mutex); pos = available_topics.find(topic); } - return pos != std::string::npos ? true : false; + bool ret = pos != std::string::npos ? true : false; +#ifdef ROS_BRIDGE_DEBUG + if ( ret ){ + std::cout << "topic " << topic << " is available" << std::endl; + } else { + std::cout << "topic " << topic << " is not available" << std::endl; + } +#endif + return ret; } void RosbridgeWsClient::advertise(const std::string &client_name, const std::string &topic, const std::string &type, const std::string &id) @@ -471,7 +466,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str return topic == std::get(td); }); if ( it_ser_top != service_topic_list.end()){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cerr << "topic: " << topic << " already advertised" << std::endl; #endif return; @@ -487,13 +482,13 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str } message = "{" + message + "}"; -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG client->on_open = [this, topic, message, client_name](std::shared_ptr connection) { #else client->on_open = [this, topic, message](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; #endif @@ -502,7 +497,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str start(client_name, client, message); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG else { std::cerr << client_name << "has not been created" << std::endl; @@ -518,7 +513,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string return topic == std::get(td); }); if ( it_ser_top == service_topic_list.end()){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cerr << "topic: " << topic << " not advertised" << std::endl; #endif return; @@ -534,13 +529,13 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string std::string client_name = "topic_unadvertiser" + topic; auto client = std::make_shared(server_port_path); -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG client->on_open = [client_name, message](std::shared_ptr connection) { #else client->on_open = [message](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; #endif @@ -569,7 +564,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum return topic == std::get(td); }); if ( it_ser_top == service_topic_list.end() ){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cerr << "topic: " << topic << " not yet advertised" << std::endl; #endif return; @@ -588,12 +583,12 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum message = "{" + message + "}"; std::shared_ptr publish_client = std::make_shared(server_port_path); -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG publish_client->on_open = [message, client_name](std::shared_ptr connection) { #else publish_client->on_open = [message, client_name](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message." << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; @@ -619,7 +614,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str return topic == std::get(td); }); if ( it_ser_top != service_topic_list.end()){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cerr << "topic: " << topic << " already advertised" << std::endl; #endif return; @@ -659,7 +654,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str client->on_message = callback; this->start(client_name, client, message); // subscribe to topic } -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG else { std::cerr << client_name << "has not been created" << std::endl; @@ -675,7 +670,7 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string return topic == std::get(td); }); if ( it_ser_top == service_topic_list.end()){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cerr << "topic: " << topic << " not advertised" << std::endl; #endif return; @@ -691,13 +686,13 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string std::string client_name = "topic_unsubscriber" + topic; auto client = std::make_shared(server_port_path); -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG client->on_open = [client_name, message](std::shared_ptr connection) { #else client->on_open = [message](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; #endif @@ -729,7 +724,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s return service == std::get(td); }); if ( it_ser_top != service_topic_list.end()){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cerr << "service: " << service << " already advertised" << std::endl; #endif return; @@ -743,7 +738,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s it_client->second->on_message = callback; start(client_name, it_client->second, message); } -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG else { std::cerr << client_name << "has not been created" << std::endl; @@ -759,7 +754,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){ return service == std::get(td); }); if ( it_ser_top == service_topic_list.end()){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cerr << "service: " << service << " not advertised" << std::endl; #endif return; @@ -771,13 +766,13 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){ std::string client_name = "service_unadvertiser" + service; auto client = std::make_shared(server_port_path); -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG client->on_open = [client_name, message](std::shared_ptr connection) { #else client->on_open = [message](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; #endif @@ -816,12 +811,12 @@ void RosbridgeWsClient::serviceResponse(const std::string &service, const std::s std::string client_name = "service_response_client" + service; std::shared_ptr service_response_client = std::make_shared(server_port_path); -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG service_response_client->on_open = [message, client_name](std::shared_ptr connection) { #else service_response_client->on_open = [message](std::shared_ptr connection) { #endif -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Opened connection" << std::endl; std::cout << client_name << ": Sending message: " << message << std::endl; #endif @@ -869,7 +864,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage else { call_service_client->on_message = [client_name](std::shared_ptr connection, std::shared_ptr in_message) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << client_name << ": Message received: " << in_message->string() << std::endl; std::cout << client_name << ": Sending close connection" << std::endl; #else @@ -884,7 +879,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage bool RosbridgeWsClient::serviceAvailable(const std::string &service) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG std::cout << "checking if service " << service << " is available" << std::endl; #endif size_t pos; @@ -892,7 +887,16 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service) std::lock_guard lk(mutex); pos = available_services.find(service); } - return pos != std::string::npos ? true : false; + bool ret = pos != std::string::npos ? true : false; +#ifdef ROS_BRIDGE_DEBUG + if ( ret ){ + std::cout << "service " << service << " is available" << std::endl; + } else { + std::cout << "service " << service << " is not available" << std::endl; + + } +#endif + return ret; } void RosbridgeWsClient::waitForService(const std::string &service) @@ -904,28 +908,27 @@ void RosbridgeWsClient::waitForService(const std::string &service) void RosbridgeWsClient::waitForService(const std::string &service, const std::function stop) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG auto s = std::chrono::high_resolution_clock::now(); long counter = 0; #endif const auto poll_interval = std::chrono::milliseconds(1000); - auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; + auto poll_time_point = std::chrono::high_resolution_clock::now(); while( !stop() ) { if ( std::chrono::high_resolution_clock::now() > poll_time_point ){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG ++counter; #endif if ( serviceAvailable(service) ){ break; - } else { - poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; } + poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; } else { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } }; -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG auto e = std::chrono::high_resolution_clock::now(); std::cout << "waitForService() " << service << " time: " << std::chrono::duration_cast(e-s).count() @@ -944,28 +947,27 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic) void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::function stop) { -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG auto s = std::chrono::high_resolution_clock::now(); long counter = 0; #endif const auto poll_interval = std::chrono::milliseconds(1000); - auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; + auto poll_time_point = std::chrono::high_resolution_clock::now(); while( !stop() ) { if ( std::chrono::high_resolution_clock::now() > poll_time_point ){ -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG ++counter; #endif if ( topicAvailable(topic) ){ break; - } else { - poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; } + poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval; } else { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } }; -#ifdef DEBUG +#ifdef ROS_BRIDGE_DEBUG auto e = std::chrono::high_resolution_clock::now(); std::cout << "waitForTopic() " << topic << " time: " << std::chrono::duration_cast(e-s).count() diff --git a/src/comm/ros_bridge/include/com_private.cpp b/src/comm/ros_bridge/include/com_private.cpp index 4d5365beb..2dceb9ee9 100644 --- a/src/comm/ros_bridge/include/com_private.cpp +++ b/src/comm/ros_bridge/include/com_private.cpp @@ -20,18 +20,10 @@ bool ros_bridge::com_private::getTopic(const ros_bridge::com_private::JsonDoc &d { if ( doc.HasMember("topic") && doc["topic"].IsString() ){ - - rapidjson::StringBuffer sb1; - rapidjson::Writer writer1(sb1); - doc.Accept(writer1); - std::cout << "getTopic doc: " << sb1.GetString() << std::endl; - rapidjson::StringBuffer sb; rapidjson::Writer writer(sb); doc["topic"].Accept(writer); topic = sb.GetString(); - - std::cout << "getTopic topic: " << sb.GetString() << std::endl; return true; } else return false; diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp index c6fc8a4cb..061bea1f0 100644 --- a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp +++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp @@ -1,5 +1,5 @@ #include "geopoint.h" std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){ - return "geometry_msgs/GeoPoint"; + return "geographic_msgs/GeoPoint"; } diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h index a28cec323..6a5e90e2a 100644 --- a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h +++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h @@ -109,9 +109,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy auto altitude = detail::getAltitude(p, traits::Type2Type()); // If T has no member altitude() replace it by 0.0; value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h index e02cf2763..c6e6865fd 100644 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h @@ -103,9 +103,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy auto z = detail::getZ(p, Type2Type()); // If T has no member z() replace it by 0. value.AddMember("z", rapidjson::Value().SetFloat(z), allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h index 2aaf3d393..e5d815c10 100644 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h @@ -57,9 +57,6 @@ bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Documen } value.AddMember("points", points, allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h index 83868b00c..49dfbff57 100644 --- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h @@ -95,9 +95,6 @@ bool _toJson(const PolygonType &poly, } value.AddMember("header", header, allocator); value.AddMember("polygon", polygon, allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h index 76693e65b..05a5802ee 100644 --- a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h +++ b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h @@ -183,10 +183,6 @@ namespace detail { detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type()); value.AddMember("likelihood", jLikelihood, allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); - return true; } diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h index 45c8f5afd..6f53c59af 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h @@ -32,9 +32,6 @@ template bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) { value.AddMember("status", std::int32_t(heartbeat.status()), allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h index fa7286c8e..12b3bbe06 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h @@ -41,9 +41,6 @@ bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document: jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator); } value.AddMember("progress", jProgress, allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.h b/src/comm/ros_bridge/include/messages/std_msgs/header.h index bd33f1200..e3a6546a0 100644 --- a/src/comm/ros_bridge/include/messages/std_msgs/header.h +++ b/src/comm/ros_bridge/include/messages/std_msgs/header.h @@ -62,9 +62,6 @@ bool toJson(const HeaderType &header, header.frameId().length(), allocator), allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.h b/src/comm/ros_bridge/include/messages/std_msgs/time.h index 3160a9743..c3f721614 100644 --- a/src/comm/ros_bridge/include/messages/std_msgs/time.h +++ b/src/comm/ros_bridge/include/messages/std_msgs/time.h @@ -37,9 +37,6 @@ bool toJson(const TimeType &time, { value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator); value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator); - rapidjson::Value jType; - jType.SetString(messageType().c_str(), allocator); - value.AddMember("type", jType, allocator); return true; } diff --git a/src/comm/ros_bridge/include/ros_bridge.h b/src/comm/ros_bridge/include/ros_bridge.h index 46d9aa59c..86e71f099 100644 --- a/src/comm/ros_bridge/include/ros_bridge.h +++ b/src/comm/ros_bridge/include/ros_bridge.h @@ -23,13 +23,16 @@ public: void advertiseService(const char* service, const char* type, const std::function &callback); + void advertiseTopic(const char* topic, + const char* type); //! @brief Start ROS bridge. void start(); //! @brief Stops ROS bridge. void reset(); + //! @return Returns true if connected to the rosbridge_server, false either. - //! @note This function can block up to 100ms. However normal execution time is smaller. + //! @note \fn calls start(). bool connected(); bool isRunning(); diff --git a/src/comm/ros_bridge/include/topic_publisher.cpp b/src/comm/ros_bridge/include/topic_publisher.cpp index 2a705d342..e9e9ce2b2 100644 --- a/src/comm/ros_bridge/include/topic_publisher.cpp +++ b/src/comm/ros_bridge/include/topic_publisher.cpp @@ -20,8 +20,6 @@ void ros_bridge::com_private::TopicPublisher::start() return; _stopped->store(false); _pThread = std::make_unique([this]{ - // Init. - std::unordered_map topicMap; // Main Loop. while( !this->_stopped->load() ){ std::unique_lock lk(this->_mutex); @@ -39,38 +37,25 @@ void ros_bridge::com_private::TopicPublisher::start() // Get topic and type from Json message and remove it. std::string topic; - assert(com_private::getTopic(*pJsonDoc, topic)); - assert(com_private::removeTopic(*pJsonDoc)); - std::string type; - assert(com_private::getType(*pJsonDoc, type)); - assert(com_private::removeType(*pJsonDoc)); - - // Check if topic must be advertised. - std::string clientName = - ros_bridge::com_private::_topicAdvertiserKey - + topic; - auto it = topicMap.find(clientName); - if ( it == topicMap.end()) { // Need to advertise topic? - topicMap.insert(std::make_pair(clientName, topic)); - this->_rbc.addClient(clientName); - this->_rbc.advertise(clientName, topic, type); - this->_rbc.waitForTopic(topic, [this]{ - return this->_stopped->load(); - }); // Wait until topic is advertised. - } + bool ret = com_private::getTopic(*pJsonDoc, topic); + assert(ret); + (void)ret; + ret = com_private::removeTopic(*pJsonDoc); + assert(ret); + (void)ret; + + // Wait for topic (Rosbridge needs some time to process a advertise() request). + this->_rbc.waitForTopic(topic, [this]{ + return this->_stopped->load(); + }); // Publish Json message. if ( !this->_stopped->load() ) this->_rbc.publish(topic, *pJsonDoc); } // while loop - - // Tidy up. - for (auto& it : topicMap){ - this->_rbc.unadvertise(it.second); - this->_rbc.removeClient(it.first); - } - +#ifdef ROS_BRIDGE_DEBUG std::cout << "TopicPublisher: publisher thread terminated." << std::endl; +#endif }); } @@ -87,7 +72,13 @@ void ros_bridge::com_private::TopicPublisher::reset() return; _pThread->join(); - lk.lock(); + lk.lock(); + // Tidy up. + for (auto& it : this->_topicMap){ + this->_rbc.unadvertise(it.first); + this->_rbc.removeClient(it.second); + } + this->_topicMap.clear(); _queue.clear(); } @@ -95,14 +86,52 @@ void ros_bridge::com_private::TopicPublisher::publish( ros_bridge::com_private::JsonDocUPtr docPtr, const char *topic) { - std::cout << "TopicPublisher \"topic\": " << topic << std::endl; + // Check if topic was advertised. + std::string t(topic); + std::unique_lock lk(this->_mutex); + auto it = this->_topicMap.find(t); + if ( it == this->_topicMap.end()) { // Not yet advertised? + lk.unlock(); +#ifdef ROS_BRIDGE_DEBUG + std::cerr << "TopicPublisher: topic " + << t << " not yet advertised" << std::endl; +#endif + return; + } + lk.unlock(); + + // Add topic information to json doc. auto &allocator = docPtr->GetAllocator(); rapidjson::Value key("topic", allocator); rapidjson::Value value(topic, allocator); docPtr->AddMember(key, value, allocator); - std::unique_lock lock(_mutex); + + lk.lock(); _queue.push_back(std::move(docPtr)); - lock.unlock(); + lk.unlock(); _cv.notify_one(); // Wake publisher thread. } +bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const char *type) +{ + std::unique_lock lk(this->_mutex); + std::string t(topic); + auto it = this->_topicMap.find(t); + if ( it == this->_topicMap.end()) { // Need to advertise topic? + std::string clientName = + std::string(ros_bridge::com_private::_topicAdvertiserKey) + + t; + this->_topicMap.insert(std::make_pair(t, clientName)); + lk.unlock(); + this->_rbc.addClient(clientName); + this->_rbc.advertise(clientName, t, type); + return true; + } else { + lk.unlock(); +#if ROS_BRIDGE_DEBUG + std::cerr << "TopicPublisher: topic " << topic << " already advertised" << std::endl; +#endif + return false; + } +} + diff --git a/src/comm/ros_bridge/include/topic_publisher.h b/src/comm/ros_bridge/include/topic_publisher.h index 8c3a21259..cbb84fb97 100644 --- a/src/comm/ros_bridge/include/topic_publisher.h +++ b/src/comm/ros_bridge/include/topic_publisher.h @@ -31,9 +31,12 @@ public: void reset(); void publish(JsonDocUPtr docPtr, const char *topic); + bool advertise(const char *topic, const char *type); private: + using TopicMap = std::unordered_map; JsonQueue _queue; + TopicMap _topicMap; std::mutex _mutex; std::shared_ptr _stopped; RosbridgeWsClient &_rbc; diff --git a/src/comm/ros_bridge/src/ros_bridge.cpp b/src/comm/ros_bridge/src/ros_bridge.cpp index cbf0bf93d..0e98af297 100644 --- a/src/comm/ros_bridge/src/ros_bridge.cpp +++ b/src/comm/ros_bridge/src/ros_bridge.cpp @@ -32,8 +32,15 @@ void ros_bridge::ROSBridge::advertiseService(const char *service, _server.advertiseService(service, type, callback); } +void ros_bridge::ROSBridge::advertiseTopic(const char *topic, const char *type) +{ + _topicPublisher.advertise(topic, type); +} + void ros_bridge::ROSBridge::start() { + if ( !_stopped->load() ) + return; _stopped->store(false); _rbc.run(); _topicPublisher.start(); @@ -44,6 +51,8 @@ void ros_bridge::ROSBridge::start() void ros_bridge::ROSBridge::reset() { auto start = std::chrono::high_resolution_clock::now(); + if ( _stopped->load() ) + return; _stopped->store(true); _topicPublisher.reset(); _topicSubscriber.reset(); @@ -56,6 +65,7 @@ void ros_bridge::ROSBridge::reset() bool ros_bridge::ROSBridge::connected() { + start(); return _rbc.connected(); } -- 2.22.0