From 96d3005b8d55f1f792eaa122289bcffda5fe0b6b Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Fri, 31 Jul 2020 18:13:17 +0200 Subject: [PATCH] 123 --- src/Snake/snake.cpp | 21 +- src/Snake/snake.h | 27 +-- src/Wima/Geometry/GeoPoint3D.cpp | 18 +- src/Wima/Geometry/GeoPoint3D.h | 18 +- src/Wima/Snake/SnakeWorker.cc | 186 +++++++++++++++--- src/Wima/Snake/SnakeWorker.h | 105 ++++++++-- src/Wima/WaypointManager/Slicer.cpp | 11 +- src/Wima/WimaController.cc | 180 ++++------------- src/Wima/WimaController.h | 11 +- src/comm/ros_bridge/include/ROSBridge.h | 1 - src/comm/ros_bridge/include/RosBridgeClient.h | 33 +++- .../ros_bridge/include/TopicPublisher.cpp | 40 ++-- src/comm/ros_bridge/include/TopicPublisher.h | 12 +- .../ros_bridge/include/TopicSubscriber.cpp | 35 ++-- src/comm/ros_bridge/include/TopicSubscriber.h | 13 +- src/comm/ros_bridge/src/ROSBridge.cpp | 5 +- 16 files changed, 407 insertions(+), 309 deletions(-) diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index 015166814..1b079432f 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -4,6 +4,13 @@ #include "clipper/clipper.hpp" #define CLIPPER_SCALE 10000 +#include "ortools/constraint_solver/routing.h" +#include "ortools/constraint_solver/routing_enums.pb.h" +#include "ortools/constraint_solver/routing_index_manager.h" +#include "ortools/constraint_solver/routing_parameters.h" + +using namespace operations_research; + #ifndef NDEBUG //#define SHOW_TIME #endif @@ -37,7 +44,7 @@ bool Scenario::addArea(Area &area) return false; } -bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea) +bool Scenario::update(double tileWidth, double tileHeight, double minTileArea) { if (!_areas2enu()) return false; @@ -301,6 +308,14 @@ bool Scenario::_calculateJoinedArea() return true; } + + +struct FlightPlan::RoutingDataModel{ + Matrix distanceMatrix; + long numVehicles; + RoutingIndexManager::NodeIndex depot; +}; + FlightPlan::FlightPlan() { @@ -356,7 +371,7 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) } size_t n1 = vertices.size(); // Generate routing model. - RoutingDataModel_t dataModel; + RoutingDataModel dataModel; Matrix connectionGraph(n1, n1); #ifdef SHOW_TIME @@ -612,7 +627,7 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt void FlightPlan::_generateRoutingModel(const BoostLineString &vertices, const BoostPolygon &polygonOffset, size_t n0, - FlightPlan::RoutingDataModel_t &dataModel, + FlightPlan::RoutingDataModel &dataModel, Matrix &graph) { #ifdef SHOW_TIME diff --git a/src/Snake/snake.h b/src/Snake/snake.h index c5ea01e1f..923b1662b 100644 --- a/src/Snake/snake.h +++ b/src/Snake/snake.h @@ -6,16 +6,9 @@ #include "snake_geometry.h" -#include "ortools/constraint_solver/routing.h" -#include "ortools/constraint_solver/routing_enums.pb.h" -#include "ortools/constraint_solver/routing_index_manager.h" -#include "ortools/constraint_solver/routing_parameters.h" - using namespace std; using namespace snake_geometry; -using namespace operations_research; - namespace snake { enum AreaType {MeasurementArea, ServiceArea, Corridor}; @@ -60,7 +53,7 @@ namespace snake { const vector &getTileCenterPoints() {return _tileCenterPoints;} const GeoPoint3D &getHomePositon() {return _homePosition;} - bool defined(double tileWidth, double tileHeight, double minTileArea); + bool update(double tileWidth, double tileHeight, double minTileArea); string errorString; @@ -104,8 +97,8 @@ namespace snake { public: FlightPlan(); - void setScenario(const Scenario &scenario) {_scenario = scenario;} - void setProgress(const vector &progress) {_progress = progress;} + void setScenario(Scenario &scenario) {_scenario = scenario;} + void setProgress(vector &progress) {_progress = progress;} const Scenario &getScenario(void) const {return _scenario;} const BoostLineString &getWaypointsENU(void) const {return _waypointsENU;} @@ -124,28 +117,22 @@ namespace snake { private: // Search Filter to speed up routing.SolveWithParameters(...); // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html - class SearchFilter : public LocalSearchFilter{ - - }; + class SearchFilter; - typedef struct{ - Matrix distanceMatrix; - long numVehicles; - RoutingIndexManager::NodeIndex depot; - }RoutingDataModel_t; + struct RoutingDataModel; bool _generateTransects(double lineDistance, double minTransectLength); void _generateRoutingModel(const BoostLineString &vertices, const BoostPolygon &polygonOffset, size_t n0, - RoutingDataModel_t &dataModel, + RoutingDataModel &dataModel, Matrix &graph); Scenario _scenario; BoostLineString _waypointsENU; GeoPoint2DList _waypoints; vector _transects; - vector _progress; + vector _progress; vector _arrivalPathIdx; vector _returnPathIdx; diff --git a/src/Wima/Geometry/GeoPoint3D.cpp b/src/Wima/Geometry/GeoPoint3D.cpp index 5f0165866..2d75e61d3 100644 --- a/src/Wima/Geometry/GeoPoint3D.cpp +++ b/src/Wima/Geometry/GeoPoint3D.cpp @@ -1,30 +1,30 @@ #include "GeoPoint3D.h" -GeoPoint3D::GeoPoint3D(QObject *parent) +GeoPoint::GeoPoint(QObject *parent) : QObject(parent), ROSGeoPoint() {} -GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent) +GeoPoint::GeoPoint(double latitude, double longitude, double altitude, QObject *parent) : QObject(parent), ROSGeoPoint(latitude, longitude, altitude) {} -GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent) +GeoPoint::GeoPoint(const GeoPoint &p, QObject *parent) : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude()) {} -GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent) +GeoPoint::GeoPoint(const ROSGeoPoint &p, QObject *parent) : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude()) {} -GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent) +GeoPoint::GeoPoint(const QGeoCoordinate &p, QObject *parent) : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude()) {} -GeoPoint3D *GeoPoint3D::Clone() const +GeoPoint *GeoPoint::Clone() const { - return new GeoPoint3D(*this); + return new GeoPoint(*this); } -GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p) +GeoPoint &GeoPoint::operator=(const GeoPoint &p) { this->setLatitude(p.latitude()); this->setLongitude(p.longitude()); @@ -32,7 +32,7 @@ GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p) return *this; } -GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p) +GeoPoint &GeoPoint::operator=(const QGeoCoordinate &p) { this->setLatitude(p.latitude()); this->setLongitude(p.longitude()); diff --git a/src/Wima/Geometry/GeoPoint3D.h b/src/Wima/Geometry/GeoPoint3D.h index 407297003..5579b63b2 100644 --- a/src/Wima/Geometry/GeoPoint3D.h +++ b/src/Wima/Geometry/GeoPoint3D.h @@ -11,27 +11,27 @@ typedef ROSBridge::MessageBaseClass ROSMsg; typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint; namespace MsgGroups = ROSBridge::MessageGroups; -class GeoPoint3D : public QObject, public ROSGeoPoint +class GeoPoint : public QObject, public ROSGeoPoint { Q_OBJECT public: typedef MsgGroups::GeoPointGroup Group; - GeoPoint3D(QObject *parent = nullptr); - GeoPoint3D(double latitude, + GeoPoint(QObject *parent = nullptr); + GeoPoint(double latitude, double longitude, double altitude, QObject *parent = nullptr); - GeoPoint3D(const GeoPoint3D& p, + GeoPoint(const GeoPoint& p, QObject *parent = nullptr); - GeoPoint3D(const ROSGeoPoint& p, + GeoPoint(const ROSGeoPoint& p, QObject *parent = nullptr); - GeoPoint3D(const QGeoCoordinate& p, + GeoPoint(const QGeoCoordinate& p, QObject *parent = nullptr); - virtual GeoPoint3D *Clone() const override; - GeoPoint3D &operator=(const GeoPoint3D&p); - GeoPoint3D &operator=(const QGeoCoordinate&p); + virtual GeoPoint *Clone() const override; + GeoPoint &operator=(const GeoPoint&p); + GeoPoint &operator=(const QGeoCoordinate&p); }; diff --git a/src/Wima/Snake/SnakeWorker.cc b/src/Wima/Snake/SnakeWorker.cc index e82bf3d64..1cbe45cba 100644 --- a/src/Wima/Snake/SnakeWorker.cc +++ b/src/Wima/Snake/SnakeWorker.cc @@ -2,58 +2,84 @@ #include +#include "snake_geometry.h" +#include "snake.h" + + + +using namespace snake; +using namespace snake_geometry; + SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent) - , _lineDistance (3) // meter - , _minTransectLength (2) // meter + , _lineDistance(3) + , _minTransectLength(2) + , _tileWidth(5) + , _tileHeight(5) + , _minTileArea(3) { } SnakeWorker::~SnakeWorker() { -} -void SnakeWorker::setScenario(const Scenario &scenario) -{ - _scenario = scenario; } -void SnakeWorker::setProgress(const QVector &progress) +WorkerResult_t &SnakeWorker::result() { - _progress.clear(); - for (auto p : progress) { - assert(p >= -1 && p <= 100); - _progress.push_back(p); - } + return _result; } -void SnakeWorker::setLineDistance(double lineDistance) +bool SnakeWorker::precondition() const { - assert(lineDistance > 0); - _lineDistance = lineDistance; -} -void SnakeWorker::setMinTransectLength(double minTransectLength) -{ - assert(minTransectLength > 0); - _minTransectLength = minTransectLength; + return _mArea.size() > 0 + && _sArea.size() > 0 + && _corridor.size() > 0 + && _progress.size() > 0; } -const WorkerResult_t &SnakeWorker::getResult() const +double SnakeWorker::lineDistance() const { - return _result; + return _lineDistance; } void SnakeWorker::run() { // Reset _result struct. - _result.success = false; - _result.errorMessage = ""; - _result.waypoints.clear(); - _result.returnPathIdx.clear(); - _result.arrivalPathIdx.clear(); + _result.clear(); + + if ( !precondition()) + return; + + Scenario scenario;// Initialize scenario. + + Area mArea; + for (auto vertex : _mArea){ + mArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()}); + } + mArea.type = AreaType::MeasurementArea; + + Area sArea; + for (auto vertex : _sArea){ + sArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()}); + } + sArea.type = AreaType::ServiceArea; + + Area corridor; + for (auto vertex : _corridor){ + corridor.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()}); + } + corridor.type = AreaType::Corridor; + + scenario.addArea(mArea); + scenario.addArea(sArea); + scenario.addArea(corridor); + + if ( !scenario.update(_tileWidth, _tileHeight, _minTileArea) ) + return; FlightPlan flightplan; - flightplan.setScenario(_scenario); + flightplan.setScenario(scenario); flightplan.setProgress(_progress); // Trying to generate flight plan. @@ -67,20 +93,116 @@ void SnakeWorker::run() _result.success = true; // Fill result struct. - auto waypoints = flightplan.getWaypoints(); + auto& waypoints = flightplan.getWaypoints(); for (auto vertex : waypoints){ - _result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0))); + _result.waypoints.append(QGeoCoordinate(vertex[0], vertex[1], 0)); } - auto arrivalPathIdx = flightplan.getArrivalPathIdx(); + auto& arrivalPathIdx = flightplan.getArrivalPathIdx(); for (auto idx : arrivalPathIdx){ _result.arrivalPathIdx.append(idx); } - auto returnPathIdx = flightplan.getReturnPathIdx(); + auto& returnPathIdx = flightplan.getReturnPathIdx(); for (auto idx : returnPathIdx){ _result.returnPathIdx.append(idx); } + + // Get tiles and origin. + auto origin = scenario.getOrigin(); + _result.origin.setLatitude(origin[0]); + _result.origin.setLongitude(origin[1]); + _result.origin.setAltitude(origin[2]); + const auto &tiles = scenario.getTiles(); + const auto &cps = scenario.getTileCenterPoints(); + for ( unsigned int i=0; i < tiles.size(); ++i ) { + const auto &tile = tiles[i]; + SnakeTile Tile; + for ( const auto &vertex : tile) { + QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]); + Tile.append(QVertex); + } + const auto ¢erPoint = cps[i]; + QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]); + Tile.setCenter(QCenterPoint); + _result.tiles.polygons().append(Tile); + _result.tileCenterPoints.append(QVariant::fromValue(QCenterPoint)); + } + + // Get local tiles. + const auto &tilesENU = scenario.getTilesENU(); + for ( unsigned int i=0; i < tilesENU.size(); ++i ) { + const auto &tile = tilesENU[i]; + Polygon2D Tile; + for ( const auto &vertex : tile.outer()) { + QPointF QVertex(vertex.get<0>(), vertex.get<1>()); + Tile.path().append(QVertex); + } + _result.tilesLocal.polygons().append(Tile); + } + } } + +double SnakeWorker::minTransectLength() const +{ + return _minTransectLength; +} + +void SnakeWorker::setMinTransectLength(double minTransectLength) +{ + if (minTransectLength > 0) + _minTransectLength = minTransectLength; +} + +double SnakeWorker::minTileArea() const +{ + return _minTileArea; +} + +void SnakeWorker::setLineDistance(double lineDistance) +{ + if (lineDistance > 0) + _lineDistance = lineDistance; +} + +double SnakeWorker::tileWidth() const +{ + return _tileWidth; +} + +void SnakeWorker::setTileWidth(double tileWidth) +{ + if (tileWidth > 0) + _tileWidth = tileWidth; +} + +double SnakeWorker::tileHeight() const +{ + return _tileHeight; +} + +void SnakeWorker::setTileHeight(double tileHeight) +{ + if (tileHeight > 0) + _tileHeight = tileHeight; +} + +void SnakeWorker::setMinTileArea(double minTileArea) +{ + if (minTileArea > 0) + _minTileArea = minTileArea; +} + +void Result::clear() +{ + this->success = false; + this->errorMessage = ""; + this->waypoints.clear(); + this->returnPathIdx.clear(); + this->arrivalPathIdx.clear(); + this->tiles.polygons().clear(); + this->tilesLocal.polygons().clear(); + this->tileCenterPoints.clear(); +} diff --git a/src/Wima/Snake/SnakeWorker.h b/src/Wima/Snake/SnakeWorker.h index 06450cec5..f7382b758 100644 --- a/src/Wima/Snake/SnakeWorker.h +++ b/src/Wima/Snake/SnakeWorker.h @@ -4,23 +4,27 @@ #include #include #include +#include +#include -#include "snake_geometry.h" -#include "snake.h" - -using namespace snake; -using namespace snake_geometry; -using namespace std; +#include "SnakeTiles.h" +#include "SnakeTilesLocal.h" typedef QList QVariantList; typedef struct Result{ - QVariantList waypoints; + QVector waypoints; + SnakeTiles tiles; + SnakeTilesLocal tilesLocal; + QVariantList tileCenterPoints; + QGeoCoordinate origin; QVector arrivalPathIdx; QVector returnPathIdx; bool success; - QString errorMessage; + QString errorMessage; + + void clear(); }WorkerResult_t; class SnakeWorker : public QThread{ @@ -30,21 +34,86 @@ public: SnakeWorker(QObject *parent = nullptr); ~SnakeWorker() override; - void setScenario (const Scenario &scenario); - void setProgress (const QVector &progress); - void setLineDistance (double lineDistance); - void setMinTransectLength (double minTransectLength); - const WorkerResult_t &getResult() const; + template class Container> + void setMeasurementArea (const Container &measurementArea); + template class Container> + void setServiceArea (const Container &serviceArea); + template class Container> + void setCorridor (const Container &corridor); + template class Container, class IntType> + void setProgress (const Container &progress); + + WorkerResult_t &result(); + + double lineDistance() const; + void setLineDistance(double lineDistance); + + double minTransectLength() const; + void setMinTransectLength(double minTransectLength); + + double minTileArea() const; + void setMinTileArea(double minTileArea); + + double tileHeight() const; + void setTileHeight(double tileHeight); + + double tileWidth() const; + void setTileWidth(double tileWidth); protected: void run() override; private: - Scenario _scenario; - vector _progress; - double _lineDistance; - double _minTransectLength; - WorkerResult_t _result; + bool precondition() const; + + + std::vector _mArea; + std::vector _sArea; + std::vector _corridor; + std::vector _progress; + double _lineDistance; + double _minTransectLength; + double _tileWidth; + double _tileHeight; + double _minTileArea; + WorkerResult_t _result; }; +template class Container> +void SnakeWorker::setCorridor(const Container &corridor) +{ + _corridor.clear(); + for (auto vertex : corridor) { + _corridor.push_back(vertex); + } +} + +template class Container> +void SnakeWorker::setMeasurementArea(const Container &measurementArea) +{ + _mArea.clear(); + for (auto vertex : measurementArea) { + _mArea.push_back(vertex); + } +} + +template class Container> +void SnakeWorker::setServiceArea(const Container &serviceArea) +{ + _sArea.clear(); + for (auto vertex : serviceArea) { + _sArea.push_back(vertex); + } +} + +template class Container, class IntType> +void SnakeWorker::setProgress(const Container &progress) +{ + _progress.clear(); + for (auto p : progress) { + assert(p >= -1 && p <= 100); + _progress.push_back(p); + } +} + diff --git a/src/Wima/WaypointManager/Slicer.cpp b/src/Wima/WaypointManager/Slicer.cpp index a880f5225..6b13a0e01 100644 --- a/src/Wima/WaypointManager/Slicer.cpp +++ b/src/Wima/WaypointManager/Slicer.cpp @@ -43,16 +43,15 @@ void Slicer::_updateIdx(long size) { _overlap = _overlap < _N ? _overlap : _N-1; - long maxStart = size-_N; - _idxStart = _idxStart <= maxStart ? _idxStart : maxStart; - _idxStart = _idxStart < 0 ? 0 : _idxStart; + _idxStart = _idxStart < size ? _idxStart : size-1; + _idxStart = _idxStart < 0 ? 0 : _idxStart; _idxEnd = _idxStart + _N - 1; _idxEnd = _idxEnd < size ? _idxEnd : size-1; - _idxNext = _idxEnd + 1 - _overlap; - _idxNext = _idxNext < 0 ? 0 : _idxNext; - _idxNext = _idxNext < size ? _idxNext : size-1; + _idxNext = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap; + _idxNext = _idxNext < 0 ? 0 : _idxNext; + _idxNext = _idxNext < size ? _idxNext : size-1; _idxPrevious = _idxStart + _overlap - _N; _idxPrevious = _idxPrevious < 0 ? 0 : _idxPrevious; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index a213b2ff1..b24552151 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -55,6 +55,7 @@ WimaController::WimaController(QObject *parent) , _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]) @@ -68,7 +69,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 +89,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); @@ -104,10 +115,6 @@ WimaController::WimaController(QObject *parent) connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit); // Snake. - connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge); - _startStopRosBridge(); - connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker); - _initStartSnakeWorker(); connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager); _switchSnakeManager(_enableSnake.rawValue()); } @@ -343,13 +350,6 @@ 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 @@ -447,80 +447,10 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) emit waypointPathChanged(); emit currentWaypointPathChanged(); + _localPlanDataValid = true; + _initStartSnakeWorker(); - // Initialize _scenario. - Area mArea; - for (auto variant : _measurementArea.path()){ - QGeoCoordinate c{variant.value()}; - mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); - } - mArea.type = AreaType::MeasurementArea; - - Area sArea; - for (auto variant : _serviceArea.path()){ - QGeoCoordinate c{variant.value()}; - sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); - } - sArea.type = AreaType::ServiceArea; - - Area corridor; - for (auto variant : _corridor.path()){ - QGeoCoordinate c{variant.value()}; - corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); - } - corridor.type = AreaType::Corridor; - - _scenario.addArea(mArea); - _scenario.addArea(sArea); - _scenario.addArea(corridor); - - // Check if scenario is defined. - if ( !_isScenarioDefinedErrorMessage() ) { - Q_ASSERT(false); - return false; - } - - { - // Get tiles and origin. - auto origin = _scenario.getOrigin(); - _snakeOrigin.setLatitude(origin[0]); - _snakeOrigin.setLongitude(origin[1]); - _snakeOrigin.setAltitude(origin[2]); - const auto &tiles = _scenario.getTiles(); - const auto &cps = _scenario.getTileCenterPoints(); - for ( unsigned int i=0; i < tiles.size(); ++i ) { - const auto &tile = tiles[i]; - SnakeTile Tile; - for ( const auto &vertex : tile) { - QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]); - Tile.append(QVertex); - } - const auto ¢erPoint = cps[i]; - QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]); - Tile.setCenter(QCenterPoint); - _snakeTiles.polygons().append(Tile); - _snakeTileCenterPoints.append(QVariant::fromValue(QCenterPoint)); - } - } - - { - // Get local tiles. - const auto &tiles = _scenario.getTilesENU(); - for ( unsigned int i=0; i < tiles.size(); ++i ) { - const auto &tile = tiles[i]; - Polygon2D Tile; - for ( const auto &vertex : tile.outer()) { - QPointF QVertex(vertex.get<0>(), vertex.get<1>()); - Tile.path().append(QVertex); - } - _snakeTilesLocal.polygons().append(Tile); - } - } - emit snakeTilesChanged(); - emit snakeTileCenterPointsChanged(); - - _localPlanDataValid = true; return true; } @@ -779,17 +709,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 +757,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,32 +816,12 @@ 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() { auto start = std::chrono::high_resolution_clock::now(); _snakeManager.clear(); - const WorkerResult_t &r = _snakeWorker.getResult(); + const WorkerResult_t &r = _snakeWorker.result(); _setSnakeCalcInProgress(false); if (!r.success) { //qgcApp()->showMessage(r.errorMessage); @@ -930,7 +838,7 @@ void WimaController::_snakeStoreWorkerResults() 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)].value()); + _snakeManager.push_back(r.waypoints[int(i)]); } _snakeManager.update(); @@ -945,15 +853,6 @@ void WimaController::_snakeStoreWorkerResults() qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms."; } -void WimaController::_startStopRosBridge() -{ - if ( _enableSnake.rawValue().toBool() ) { - _pRosBridge->start(); - } else { - _pRosBridge->reset(); - } -} - void WimaController::_initStartSnakeWorker() { if ( !_enableSnake.rawValue().toBool() ) @@ -965,10 +864,15 @@ void WimaController::_initStartSnakeWorker() } // Initialize _snakeWorker. - _snakeWorker.setScenario(_scenario); + _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. diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 6fd11ca0f..edf33e85c 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -338,10 +338,7 @@ private slots: void _smartRTLCleanUp (bool flying); // Snake. void _setSnakeCalcInProgress (bool inProgress); - bool _isScenarioDefined (void); - bool _isScenarioDefinedErrorMessage (void); void _snakeStoreWorkerResults (); - void _startStopRosBridge (); void _initStartSnakeWorker (); void _switchSnakeManager (QVariant variant); // Periodic tasks. @@ -357,7 +354,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 +368,8 @@ private: WaypointManager::DefaultManager _snakeManager; WaypointManager::RTLManager _rtlManager; WaypointManager::ManagerBase *_currentManager; + using ManagerList = QList; + ManagerList _managerList; // Settings Facts. QMap _metaDataMap; @@ -403,11 +401,8 @@ private: // Snake bool _snakeCalcInProgress; - bool _snakeRecalcNecessary; - bool _scenarioDefinedBool; SnakeWorker _snakeWorker; - Scenario _scenario; - ::GeoPoint3D _snakeOrigin; + GeoPoint _snakeOrigin; SnakeTiles _snakeTiles; // tiles SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system QVariantList _snakeTileCenterPoints; diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h index 589101a99..6ee6f94e9 100644 --- a/src/comm/ros_bridge/include/ROSBridge.h +++ b/src/comm/ros_bridge/include/ROSBridge.h @@ -45,7 +45,6 @@ private: JsonFactory _jsonFactory; CasePacker _casePacker; RosbridgeWsClient _rbc; - std::mutex _rbcMutex; ComPrivate::TopicPublisher _topicPublisher; ComPrivate::TopicSubscriber _topicSubscriber; diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index 039e60f5e..d2e70ae8c 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -17,6 +17,7 @@ #include #include #include +#include using WsClient = SimpleWeb::SocketClient; using InMessage = std::function, std::shared_ptr)>; @@ -25,6 +26,7 @@ class RosbridgeWsClient { std::string server_port_path; std::unordered_map> client_map; + std::mutex map_mutex; // Starts the client. void start(const std::string& client_name, std::shared_ptr client, const std::string& message) @@ -86,9 +88,6 @@ class RosbridgeWsClient }); client_thread.detach(); - - // 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)); } public: @@ -99,6 +98,8 @@ public: ~RosbridgeWsClient() { + + std::lock_guard lk(map_mutex); // neccessary? for (auto& client : client_map) { client.second->stop(); @@ -136,6 +137,8 @@ public: // 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()) { @@ -151,6 +154,8 @@ public: 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()) { @@ -161,6 +166,8 @@ public: 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()) { @@ -183,12 +190,22 @@ public: 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()) { - it->second->stop(); - it->second.reset(); + // Stop the client asynchronously in 100 ms. + // This is to ensure, that all threads involving 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 ROS_BRIDGE_CLIENT_DEBUG std::cout << client_name << " has been removed" << std::endl; #endif @@ -205,6 +222,8 @@ public: // 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()) { @@ -258,6 +277,8 @@ public: 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()) { @@ -302,6 +323,8 @@ public: 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()) { diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp index 18cda8de9..1e44a7a74 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ b/src/comm/ros_bridge/include/TopicPublisher.cpp @@ -8,20 +8,17 @@ struct ROSBridge::ComPrivate::ThreadData { const ROSBridge::CasePacker &casePacker; RosbridgeWsClient &rbc; - std::mutex &rbcMutex; ROSBridge::ComPrivate::JsonQueue &queue; std::mutex &queueMutex; const std::atomic &running; std::condition_variable &cv; }; -ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker, - RosbridgeWsClient *rbc, - std::mutex *rbcMutex) : +ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker, + RosbridgeWsClient &rbc) : _running(false) , _casePacker(casePacker) , _rbc(rbc) - , _rbcMutex(rbcMutex) { } @@ -37,9 +34,8 @@ void ROSBridge::ComPrivate::TopicPublisher::start() return; _running.store(true); ROSBridge::ComPrivate::ThreadData data{ - *_casePacker, - *_rbc, - *_rbcMutex, + _casePacker, + _rbc, _queue, _queueMutex, _running, @@ -64,11 +60,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data { // Init. ClientMap clientMap; -// { -// std::lock_guard lk(data.rbcMutex); -// data.rbc.addClient(ROSBridge::ComPrivate::_topicAdvertiserKey); -// data.rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey); -// } // Main Loop. while(data.running.load()){ std::unique_lock lk(data.queueMutex); @@ -101,27 +92,18 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data std::cout << clientName << ";" << tag.topic() << ";" << tag.messageType() << ";" << std::endl; - { - std::lock_guard lk(data.rbcMutex); - data.rbc.addClient(clientName); - data.rbc.advertise(clientName, - tag.topic(), - tag.messageType() ); - } + data.rbc.addClient(clientName); + data.rbc.advertise(clientName, + tag.topic(), + tag.messageType() ); } // Publish Json message. - { - std::lock_guard lk(data.rbcMutex); - data.rbc.publish(tag.topic(), *pJsonDoc.get()); - } + data.rbc.publish(tag.topic(), *pJsonDoc.get()); } // while loop // Tidy up. - { - std::lock_guard lk(data.rbcMutex); - for (auto& it : clientMap) - data.rbc.removeClient(it.second); - } + for (auto& it : clientMap) + data.rbc.removeClient(it.second); } diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/TopicPublisher.h index ce7e14285..1c9dc680a 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.h +++ b/src/comm/ros_bridge/include/TopicPublisher.h @@ -24,9 +24,8 @@ class TopicPublisher public: TopicPublisher() = delete; - TopicPublisher(CasePacker *casePacker, - RosbridgeWsClient *rbc, - std::mutex *rbcMutex); + TopicPublisher(CasePacker &casePacker, + RosbridgeWsClient &rbc); ~TopicPublisher(); @@ -46,7 +45,7 @@ public: template void publish(const T &msg, const std::string &topic){ - JsonDocUPtr docPtr(_casePacker->pack(msg, topic)); + JsonDocUPtr docPtr(_casePacker.pack(msg, topic)); publish(std::move(docPtr)); } @@ -54,9 +53,8 @@ private: JsonQueue _queue; std::mutex _queueMutex; std::atomic _running; - CasePacker *_casePacker; - RosbridgeWsClient *_rbc; - std::mutex *_rbcMutex; + CasePacker &_casePacker; + RosbridgeWsClient &_rbc; CondVar _cv; ThreadPtr _pThread; }; diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/TopicSubscriber.cpp index dce6371ec..7a1d8311f 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.cpp +++ b/src/comm/ros_bridge/include/TopicSubscriber.cpp @@ -1,11 +1,10 @@ #include "TopicSubscriber.h" -ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(ROSBridge::CasePacker *casePacker, - RosbridgeWsClient *rbc, std::mutex *rbcMutex) : +ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker, + RosbridgeWsClient &rbc) : _casePacker(casePacker) , _rbc(rbc) - , _rbcMutex(rbcMutex) , _running(false) { @@ -26,14 +25,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::reset() if ( _running ){ _running = false; { - std::lock_guard lk(*_rbcMutex); for (std::string clientName : _clientList){ - _rbc->removeClient(clientName); + _rbc.removeClient(clientName); } } { - std::lock_guard lk(_callbackMap.mutex); - _callbackMap.map.clear(); + std::lock_guard lk(_callbackMapStruct.mutex); + _callbackMapStruct.map.clear(); } _clientList.clear(); } @@ -52,8 +50,8 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( HashType hash = getHash(clientName); { - std::lock_guard lk(_callbackMap.mutex); - auto ret = _callbackMap.map.insert(std::make_pair(hash, callback)); // + std::lock_guard lk(_callbackMapStruct.mutex); + auto ret = _callbackMapStruct.map.insert(std::make_pair(hash, callback)); // if ( !ret.second ) return false; // Topic subscription already present. @@ -62,7 +60,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( using namespace std::placeholders; auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback, hash, - std::ref(_callbackMap), + std::ref(_callbackMapStruct), _1, _2); // std::cout << std::endl; @@ -70,11 +68,20 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( // std::cout << "client name: " << clientName << std::endl; // std::cout << "topic: " << topic << std::endl; { - std::lock_guard lk(*_rbcMutex); - _rbc->addClient(clientName); - _rbc->subscribe(clientName, + auto start = std::chrono::high_resolution_clock::now(); + _rbc.addClient(clientName); + auto end = std::chrono::high_resolution_clock::now(); + std::cout << "add client time: " + << std::chrono::duration_cast(end-start).count() + << " ms" << std::endl; + start = std::chrono::high_resolution_clock::now(); + _rbc.subscribe(clientName, topic, f ); + end = std::chrono::high_resolution_clock::now(); + std::cout << "subscribe time: " + << std::chrono::duration_cast(end-start).count() + << " ms" << std::endl; } return true; @@ -84,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( using WsClient = SimpleWeb::SocketClient; void ROSBridge::ComPrivate::subscriberCallback( const HashType &hash, - ROSBridge::ComPrivate::CallbackMapWrapper &mapWrapper, + ROSBridge::ComPrivate::MapStruct &mapWrapper, std::shared_ptr, std::shared_ptr in_message) { diff --git a/src/comm/ros_bridge/include/TopicSubscriber.h b/src/comm/ros_bridge/include/TopicSubscriber.h index 4ce462313..585e36477 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.h +++ b/src/comm/ros_bridge/include/TopicSubscriber.h @@ -10,7 +10,7 @@ namespace ComPrivate { typedef std::function CallbackType; -struct CallbackMapWrapper{ +struct MapStruct{ typedef std::map Map; Map map; std::mutex mutex; @@ -22,7 +22,7 @@ class TopicSubscriber public: TopicSubscriber() = delete; - TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc, std::mutex *rbcMutex); + TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc); ~TopicSubscriber(); //! @brief Starts the subscriber. @@ -40,16 +40,15 @@ private: - CasePacker *_casePacker; - RosbridgeWsClient *_rbc; - std::mutex *_rbcMutex; - CallbackMapWrapper _callbackMap; + CasePacker &_casePacker; + RosbridgeWsClient &_rbc; + MapStruct _callbackMapStruct; ClientList _clientList; bool _running; }; void subscriberCallback(const HashType &hash, - CallbackMapWrapper &mapWrapper, + MapStruct &mapWrapper, std::shared_ptr /*connection*/, std::shared_ptr in_message); } // namespace ComPrivate diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp index 9b8d07fa5..32d8ab791 100644 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ b/src/comm/ros_bridge/src/ROSBridge.cpp @@ -3,8 +3,8 @@ ROSBridge::ROSBridge::ROSBridge() : _casePacker(&_typeFactory, &_jsonFactory) , _rbc("localhost:9090") - , _topicPublisher(&_casePacker, &_rbc, &_rbcMutex) - , _topicSubscriber(&_casePacker, &_rbc, &_rbcMutex) + , _topicPublisher(_casePacker, _rbc) + , _topicSubscriber(_casePacker, _rbc) { } @@ -38,7 +38,6 @@ void ROSBridge::ROSBridge::reset() bool ROSBridge::ROSBridge::connected() { - std::lock_guard lk(_rbcMutex); return _rbc.connected(); } -- 2.22.0