Commit 96d3005b authored by Valentin Platzgummer's avatar Valentin Platzgummer

123

parent 07a48251
...@@ -4,6 +4,13 @@ ...@@ -4,6 +4,13 @@
#include "clipper/clipper.hpp" #include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000 #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 #ifndef NDEBUG
//#define SHOW_TIME //#define SHOW_TIME
#endif #endif
...@@ -37,7 +44,7 @@ bool Scenario::addArea(Area &area) ...@@ -37,7 +44,7 @@ bool Scenario::addArea(Area &area)
return false; return false;
} }
bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea) bool Scenario::update(double tileWidth, double tileHeight, double minTileArea)
{ {
if (!_areas2enu()) if (!_areas2enu())
return false; return false;
...@@ -301,6 +308,14 @@ bool Scenario::_calculateJoinedArea() ...@@ -301,6 +308,14 @@ bool Scenario::_calculateJoinedArea()
return true; return true;
} }
struct FlightPlan::RoutingDataModel{
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
};
FlightPlan::FlightPlan() FlightPlan::FlightPlan()
{ {
...@@ -356,7 +371,7 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) ...@@ -356,7 +371,7 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
} }
size_t n1 = vertices.size(); size_t n1 = vertices.size();
// Generate routing model. // Generate routing model.
RoutingDataModel_t dataModel; RoutingDataModel dataModel;
Matrix<double> connectionGraph(n1, n1); Matrix<double> connectionGraph(n1, n1);
#ifdef SHOW_TIME #ifdef SHOW_TIME
...@@ -612,7 +627,7 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt ...@@ -612,7 +627,7 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
void FlightPlan::_generateRoutingModel(const BoostLineString &vertices, void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset, const BoostPolygon &polygonOffset,
size_t n0, size_t n0,
FlightPlan::RoutingDataModel_t &dataModel, FlightPlan::RoutingDataModel &dataModel,
Matrix<double> &graph) Matrix<double> &graph)
{ {
#ifdef SHOW_TIME #ifdef SHOW_TIME
......
...@@ -6,16 +6,9 @@ ...@@ -6,16 +6,9 @@
#include "snake_geometry.h" #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 std;
using namespace snake_geometry; using namespace snake_geometry;
using namespace operations_research;
namespace snake { namespace snake {
enum AreaType {MeasurementArea, ServiceArea, Corridor}; enum AreaType {MeasurementArea, ServiceArea, Corridor};
...@@ -60,7 +53,7 @@ namespace snake { ...@@ -60,7 +53,7 @@ namespace snake {
const vector<GeoPoint3D> &getTileCenterPoints() {return _tileCenterPoints;} const vector<GeoPoint3D> &getTileCenterPoints() {return _tileCenterPoints;}
const GeoPoint3D &getHomePositon() {return _homePosition;} const GeoPoint3D &getHomePositon() {return _homePosition;}
bool defined(double tileWidth, double tileHeight, double minTileArea); bool update(double tileWidth, double tileHeight, double minTileArea);
string errorString; string errorString;
...@@ -104,8 +97,8 @@ namespace snake { ...@@ -104,8 +97,8 @@ namespace snake {
public: public:
FlightPlan(); FlightPlan();
void setScenario(const Scenario &scenario) {_scenario = scenario;} void setScenario(Scenario &scenario) {_scenario = scenario;}
void setProgress(const vector<int8_t> &progress) {_progress = progress;} void setProgress(vector<int> &progress) {_progress = progress;}
const Scenario &getScenario(void) const {return _scenario;} const Scenario &getScenario(void) const {return _scenario;}
const BoostLineString &getWaypointsENU(void) const {return _waypointsENU;} const BoostLineString &getWaypointsENU(void) const {return _waypointsENU;}
...@@ -124,28 +117,22 @@ namespace snake { ...@@ -124,28 +117,22 @@ namespace snake {
private: private:
// Search Filter to speed up routing.SolveWithParameters(...); // 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 // 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{ struct RoutingDataModel;
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
}RoutingDataModel_t;
bool _generateTransects(double lineDistance, double minTransectLength); bool _generateTransects(double lineDistance, double minTransectLength);
void _generateRoutingModel(const BoostLineString &vertices, void _generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset, const BoostPolygon &polygonOffset,
size_t n0, size_t n0,
RoutingDataModel_t &dataModel, RoutingDataModel &dataModel,
Matrix<double> &graph); Matrix<double> &graph);
Scenario _scenario; Scenario _scenario;
BoostLineString _waypointsENU; BoostLineString _waypointsENU;
GeoPoint2DList _waypoints; GeoPoint2DList _waypoints;
vector<BoostLineString> _transects; vector<BoostLineString> _transects;
vector<int8_t> _progress; vector<int> _progress;
vector<uint64_t> _arrivalPathIdx; vector<uint64_t> _arrivalPathIdx;
vector<uint64_t> _returnPathIdx; vector<uint64_t> _returnPathIdx;
......
#include "GeoPoint3D.h" #include "GeoPoint3D.h"
GeoPoint3D::GeoPoint3D(QObject *parent) GeoPoint::GeoPoint(QObject *parent)
: QObject(parent), ROSGeoPoint() {} : 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) : 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()) : 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()) : 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()) : 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->setLatitude(p.latitude());
this->setLongitude(p.longitude()); this->setLongitude(p.longitude());
...@@ -32,7 +32,7 @@ GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p) ...@@ -32,7 +32,7 @@ GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
return *this; return *this;
} }
GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p) GeoPoint &GeoPoint::operator=(const QGeoCoordinate &p)
{ {
this->setLatitude(p.latitude()); this->setLatitude(p.latitude());
this->setLongitude(p.longitude()); this->setLongitude(p.longitude());
......
...@@ -11,27 +11,27 @@ ...@@ -11,27 +11,27 @@
typedef ROSBridge::MessageBaseClass ROSMsg; typedef ROSBridge::MessageBaseClass ROSMsg;
typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint; typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
namespace MsgGroups = ROSBridge::MessageGroups; namespace MsgGroups = ROSBridge::MessageGroups;
class GeoPoint3D : public QObject, public ROSGeoPoint class GeoPoint : public QObject, public ROSGeoPoint
{ {
Q_OBJECT Q_OBJECT
public: public:
typedef MsgGroups::GeoPointGroup Group; typedef MsgGroups::GeoPointGroup Group;
GeoPoint3D(QObject *parent = nullptr); GeoPoint(QObject *parent = nullptr);
GeoPoint3D(double latitude, GeoPoint(double latitude,
double longitude, double longitude,
double altitude, double altitude,
QObject *parent = nullptr); QObject *parent = nullptr);
GeoPoint3D(const GeoPoint3D& p, GeoPoint(const GeoPoint& p,
QObject *parent = nullptr); QObject *parent = nullptr);
GeoPoint3D(const ROSGeoPoint& p, GeoPoint(const ROSGeoPoint& p,
QObject *parent = nullptr); QObject *parent = nullptr);
GeoPoint3D(const QGeoCoordinate& p, GeoPoint(const QGeoCoordinate& p,
QObject *parent = nullptr); QObject *parent = nullptr);
virtual GeoPoint3D *Clone() const override; virtual GeoPoint *Clone() const override;
GeoPoint3D &operator=(const GeoPoint3D&p); GeoPoint &operator=(const GeoPoint&p);
GeoPoint3D &operator=(const QGeoCoordinate&p); GeoPoint &operator=(const QGeoCoordinate&p);
}; };
......
...@@ -2,58 +2,84 @@ ...@@ -2,58 +2,84 @@
#include <QGeoCoordinate> #include <QGeoCoordinate>
#include "snake_geometry.h"
#include "snake.h"
using namespace snake;
using namespace snake_geometry;
SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent) SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
, _lineDistance (3) // meter , _lineDistance(3)
, _minTransectLength (2) // meter , _minTransectLength(2)
, _tileWidth(5)
, _tileHeight(5)
, _minTileArea(3)
{ {
} }
SnakeWorker::~SnakeWorker() SnakeWorker::~SnakeWorker()
{ {
}
void SnakeWorker::setScenario(const Scenario &scenario)
{
_scenario = scenario;
} }
void SnakeWorker::setProgress(const QVector<int> &progress) WorkerResult_t &SnakeWorker::result()
{ {
_progress.clear(); return _result;
for (auto p : progress) {
assert(p >= -1 && p <= 100);
_progress.push_back(p);
}
} }
void SnakeWorker::setLineDistance(double lineDistance) bool SnakeWorker::precondition() const
{ {
assert(lineDistance > 0);
_lineDistance = lineDistance;
}
void SnakeWorker::setMinTransectLength(double minTransectLength) return _mArea.size() > 0
{ && _sArea.size() > 0
assert(minTransectLength > 0); && _corridor.size() > 0
_minTransectLength = minTransectLength; && _progress.size() > 0;
} }
const WorkerResult_t &SnakeWorker::getResult() const double SnakeWorker::lineDistance() const
{ {
return _result; return _lineDistance;
} }
void SnakeWorker::run() void SnakeWorker::run()
{ {
// Reset _result struct. // Reset _result struct.
_result.success = false; _result.clear();
_result.errorMessage = "";
_result.waypoints.clear(); if ( !precondition())
_result.returnPathIdx.clear(); return;
_result.arrivalPathIdx.clear();
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 flightplan;
flightplan.setScenario(_scenario); flightplan.setScenario(scenario);
flightplan.setProgress(_progress); flightplan.setProgress(_progress);
// Trying to generate flight plan. // Trying to generate flight plan.
...@@ -67,20 +93,116 @@ void SnakeWorker::run() ...@@ -67,20 +93,116 @@ void SnakeWorker::run()
_result.success = true; _result.success = true;
// Fill result struct. // Fill result struct.
auto waypoints = flightplan.getWaypoints(); auto& waypoints = flightplan.getWaypoints();
for (auto vertex : waypoints){ 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){ for (auto idx : arrivalPathIdx){
_result.arrivalPathIdx.append(idx); _result.arrivalPathIdx.append(idx);
} }
auto returnPathIdx = flightplan.getReturnPathIdx(); auto& returnPathIdx = flightplan.getReturnPathIdx();
for (auto idx : returnPathIdx){ for (auto idx : returnPathIdx){
_result.returnPathIdx.append(idx); _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 &centerPoint = 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();
} }
...@@ -4,23 +4,27 @@ ...@@ -4,23 +4,27 @@
#include <QVariant> #include <QVariant>
#include <QThread> #include <QThread>
#include <QVector> #include <QVector>
#include <QGeoCoordinate>
#include <vector>
#include "snake_geometry.h" #include "SnakeTiles.h"
#include "snake.h" #include "SnakeTilesLocal.h"
using namespace snake;
using namespace snake_geometry;
using namespace std;
typedef QList<QVariant> QVariantList; typedef QList<QVariant> QVariantList;
typedef struct Result{ typedef struct Result{
QVariantList waypoints; QVector<QGeoCoordinate> waypoints;
SnakeTiles tiles;
SnakeTilesLocal tilesLocal;
QVariantList tileCenterPoints;
QGeoCoordinate origin;
QVector<unsigned long> arrivalPathIdx; QVector<unsigned long> arrivalPathIdx;
QVector<unsigned long> returnPathIdx; QVector<unsigned long> returnPathIdx;
bool success; bool success;
QString errorMessage; QString errorMessage;
void clear();
}WorkerResult_t; }WorkerResult_t;
class SnakeWorker : public QThread{ class SnakeWorker : public QThread{
...@@ -30,21 +34,86 @@ public: ...@@ -30,21 +34,86 @@ public:
SnakeWorker(QObject *parent = nullptr); SnakeWorker(QObject *parent = nullptr);
~SnakeWorker() override; ~SnakeWorker() override;
void setScenario (const Scenario &scenario);
void setProgress (const QVector<int> &progress);
void setLineDistance (double lineDistance);
void setMinTransectLength (double minTransectLength);
const WorkerResult_t &getResult() const; template<template<class, class...> class Container>
void setMeasurementArea (const Container<QGeoCoordinate> &measurementArea);
template<template<class, class...> class Container>
void setServiceArea (const Container<QGeoCoordinate> &serviceArea);
template<template<class, class...> class Container>
void setCorridor (const Container<QGeoCoordinate> &corridor);
template<template<class, class...> class Container, class IntType>
void setProgress (const Container<IntType> &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: protected:
void run() override; void run() override;
private: private:
Scenario _scenario; bool precondition() const;
vector<int8_t> _progress;
std::vector<QGeoCoordinate> _mArea;
std::vector<QGeoCoordinate> _sArea;
std::vector<QGeoCoordinate> _corridor;
std::vector<int> _progress;
double _lineDistance; double _lineDistance;
double _minTransectLength; double _minTransectLength;
double _tileWidth;
double _tileHeight;
double _minTileArea;
WorkerResult_t _result; WorkerResult_t _result;
}; };
template<template<class, class...> class Container>
void SnakeWorker::setCorridor(const Container<QGeoCoordinate> &corridor)
{
_corridor.clear();
for (auto vertex : corridor) {
_corridor.push_back(vertex);
}
}
template<template<class, class...> class Container>
void SnakeWorker::setMeasurementArea(const Container<QGeoCoordinate> &measurementArea)
{
_mArea.clear();
for (auto vertex : measurementArea) {
_mArea.push_back(vertex);
}
}
template<template<class, class...> class Container>
void SnakeWorker::setServiceArea(const Container<QGeoCoordinate> &serviceArea)
{
_sArea.clear();
for (auto vertex : serviceArea) {
_sArea.push_back(vertex);
}
}
template<template<class, class...> class Container, class IntType>
void SnakeWorker::setProgress(const Container<IntType> &progress)
{
_progress.clear();
for (auto p : progress) {
assert(p >= -1 && p <= 100);
_progress.push_back(p);
}
}
...@@ -43,14 +43,13 @@ void Slicer::_updateIdx(long size) ...@@ -43,14 +43,13 @@ void Slicer::_updateIdx(long size)
{ {
_overlap = _overlap < _N ? _overlap : _N-1; _overlap = _overlap < _N ? _overlap : _N-1;
long maxStart = size-_N; _idxStart = _idxStart < size ? _idxStart : size-1;
_idxStart = _idxStart <= maxStart ? _idxStart : maxStart;
_idxStart = _idxStart < 0 ? 0 : _idxStart; _idxStart = _idxStart < 0 ? 0 : _idxStart;
_idxEnd = _idxStart + _N - 1; _idxEnd = _idxStart + _N - 1;
_idxEnd = _idxEnd < size ? _idxEnd : size-1; _idxEnd = _idxEnd < size ? _idxEnd : size-1;
_idxNext = _idxEnd + 1 - _overlap; _idxNext = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap;
_idxNext = _idxNext < 0 ? 0 : _idxNext; _idxNext = _idxNext < 0 ? 0 : _idxNext;
_idxNext = _idxNext < size ? _idxNext : size-1; _idxNext = _idxNext < size ? _idxNext : size-1;
......
This diff is collapsed.
...@@ -338,10 +338,7 @@ private slots: ...@@ -338,10 +338,7 @@ private slots:
void _smartRTLCleanUp (bool flying); void _smartRTLCleanUp (bool flying);
// Snake. // Snake.
void _setSnakeCalcInProgress (bool inProgress); void _setSnakeCalcInProgress (bool inProgress);
bool _isScenarioDefined (void);
bool _isScenarioDefinedErrorMessage (void);
void _snakeStoreWorkerResults (); void _snakeStoreWorkerResults ();
void _startStopRosBridge ();
void _initStartSnakeWorker (); void _initStartSnakeWorker ();
void _switchSnakeManager (QVariant variant); void _switchSnakeManager (QVariant variant);
// Periodic tasks. // Periodic tasks.
...@@ -357,7 +354,6 @@ private: ...@@ -357,7 +354,6 @@ private:
MissionController *_missionController; MissionController *_missionController;
// Wima Data. // Wima Data.
QmlObjectListModel _areas; // contains all visible areas QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area WimaMeasurementAreaData _measurementArea; // measurement area
...@@ -372,6 +368,8 @@ private: ...@@ -372,6 +368,8 @@ private:
WaypointManager::DefaultManager _snakeManager; WaypointManager::DefaultManager _snakeManager;
WaypointManager::RTLManager _rtlManager; WaypointManager::RTLManager _rtlManager;
WaypointManager::ManagerBase *_currentManager; WaypointManager::ManagerBase *_currentManager;
using ManagerList = QList<WaypointManager::ManagerBase*>;
ManagerList _managerList;
// Settings Facts. // Settings Facts.
QMap<QString, FactMetaData*> _metaDataMap; QMap<QString, FactMetaData*> _metaDataMap;
...@@ -403,11 +401,8 @@ private: ...@@ -403,11 +401,8 @@ private:
// Snake // Snake
bool _snakeCalcInProgress; bool _snakeCalcInProgress;
bool _snakeRecalcNecessary;
bool _scenarioDefinedBool;
SnakeWorker _snakeWorker; SnakeWorker _snakeWorker;
Scenario _scenario; GeoPoint _snakeOrigin;
::GeoPoint3D _snakeOrigin;
SnakeTiles _snakeTiles; // tiles SnakeTiles _snakeTiles; // tiles
SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system
QVariantList _snakeTileCenterPoints; QVariantList _snakeTileCenterPoints;
......
...@@ -45,7 +45,6 @@ private: ...@@ -45,7 +45,6 @@ private:
JsonFactory _jsonFactory; JsonFactory _jsonFactory;
CasePacker _casePacker; CasePacker _casePacker;
RosbridgeWsClient _rbc; RosbridgeWsClient _rbc;
std::mutex _rbcMutex;
ComPrivate::TopicPublisher _topicPublisher; ComPrivate::TopicPublisher _topicPublisher;
ComPrivate::TopicSubscriber _topicSubscriber; ComPrivate::TopicSubscriber _topicSubscriber;
......
...@@ -17,6 +17,7 @@ ...@@ -17,6 +17,7 @@
#include <thread> #include <thread>
#include <future> #include <future>
#include <functional> #include <functional>
#include <mutex>
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>; using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>; using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
...@@ -25,6 +26,7 @@ class RosbridgeWsClient ...@@ -25,6 +26,7 @@ class RosbridgeWsClient
{ {
std::string server_port_path; std::string server_port_path;
std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map; std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
std::mutex map_mutex;
// Starts the client. // Starts the client.
void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message) void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
...@@ -86,9 +88,6 @@ class RosbridgeWsClient ...@@ -86,9 +88,6 @@ class RosbridgeWsClient
}); });
client_thread.detach(); 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: public:
...@@ -99,6 +98,8 @@ public: ...@@ -99,6 +98,8 @@ public:
~RosbridgeWsClient() ~RosbridgeWsClient()
{ {
std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
for (auto& client : client_map) for (auto& client : client_map)
{ {
client.second->stop(); client.second->stop();
...@@ -136,6 +137,8 @@ public: ...@@ -136,6 +137,8 @@ public:
// Adds a client to the client_map. // Adds a client to the client_map.
void addClient(const std::string& client_name) void addClient(const std::string& client_name)
{ {
std::lock_guard<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name); std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it == client_map.end()) if (it == client_map.end())
{ {
...@@ -151,6 +154,8 @@ public: ...@@ -151,6 +154,8 @@ public:
std::shared_ptr<WsClient> getClient(const std::string& client_name) std::shared_ptr<WsClient> getClient(const std::string& client_name)
{ {
std::lock_guard<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name); std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end()) if (it != client_map.end())
{ {
...@@ -161,6 +166,8 @@ public: ...@@ -161,6 +166,8 @@ public:
void stopClient(const std::string& client_name) void stopClient(const std::string& client_name)
{ {
std::lock_guard<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name); std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end()) if (it != client_map.end())
{ {
...@@ -183,12 +190,22 @@ public: ...@@ -183,12 +190,22 @@ public:
void removeClient(const std::string& client_name) void removeClient(const std::string& client_name)
{ {
std::lock_guard<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name); std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end()) if (it != client_map.end())
{ {
it->second->stop(); // Stop the client asynchronously in 100 ms.
it->second.reset(); // This is to ensure, that all threads involving client have been launched.
std::thread t([](std::shared_ptr<WsClient> client){
std::this_thread::sleep_for(std::chrono::milliseconds(100));
client->stop();
client.reset();
},
it->second /*lambda param*/ );
client_map.erase(it); client_map.erase(it);
t.detach();
#ifdef ROS_BRIDGE_CLIENT_DEBUG #ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << client_name << " has been removed" << std::endl; std::cout << client_name << " has been removed" << std::endl;
#endif #endif
...@@ -205,6 +222,8 @@ public: ...@@ -205,6 +222,8 @@ public:
// One client per topic! // One client per topic!
void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "") void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "")
{ {
std::lock_guard<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name); std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end()) if (it != client_map.end())
{ {
...@@ -258,6 +277,8 @@ public: ...@@ -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 = "") 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<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name); std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end()) if (it != client_map.end())
{ {
...@@ -302,6 +323,8 @@ public: ...@@ -302,6 +323,8 @@ public:
void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback) void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback)
{ {
std::lock_guard<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name); std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end()) if (it != client_map.end())
{ {
......
...@@ -8,20 +8,17 @@ struct ROSBridge::ComPrivate::ThreadData ...@@ -8,20 +8,17 @@ struct ROSBridge::ComPrivate::ThreadData
{ {
const ROSBridge::CasePacker &casePacker; const ROSBridge::CasePacker &casePacker;
RosbridgeWsClient &rbc; RosbridgeWsClient &rbc;
std::mutex &rbcMutex;
ROSBridge::ComPrivate::JsonQueue &queue; ROSBridge::ComPrivate::JsonQueue &queue;
std::mutex &queueMutex; std::mutex &queueMutex;
const std::atomic<bool> &running; const std::atomic<bool> &running;
std::condition_variable &cv; std::condition_variable &cv;
}; };
ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker, ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
RosbridgeWsClient *rbc, RosbridgeWsClient &rbc) :
std::mutex *rbcMutex) :
_running(false) _running(false)
, _casePacker(casePacker) , _casePacker(casePacker)
, _rbc(rbc) , _rbc(rbc)
, _rbcMutex(rbcMutex)
{ {
} }
...@@ -37,9 +34,8 @@ void ROSBridge::ComPrivate::TopicPublisher::start() ...@@ -37,9 +34,8 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
return; return;
_running.store(true); _running.store(true);
ROSBridge::ComPrivate::ThreadData data{ ROSBridge::ComPrivate::ThreadData data{
*_casePacker, _casePacker,
*_rbc, _rbc,
*_rbcMutex,
_queue, _queue,
_queueMutex, _queueMutex,
_running, _running,
...@@ -64,11 +60,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data ...@@ -64,11 +60,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
{ {
// Init. // Init.
ClientMap clientMap; ClientMap clientMap;
// {
// std::lock_guard<std::mutex> lk(data.rbcMutex);
// data.rbc.addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
// data.rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
// }
// Main Loop. // Main Loop.
while(data.running.load()){ while(data.running.load()){
std::unique_lock<std::mutex> lk(data.queueMutex); std::unique_lock<std::mutex> lk(data.queueMutex);
...@@ -101,27 +92,18 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data ...@@ -101,27 +92,18 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
std::cout << clientName << ";" std::cout << clientName << ";"
<< tag.topic() << ";" << tag.topic() << ";"
<< tag.messageType() << ";" << std::endl; << tag.messageType() << ";" << std::endl;
{
std::lock_guard<std::mutex> lk(data.rbcMutex);
data.rbc.addClient(clientName); data.rbc.addClient(clientName);
data.rbc.advertise(clientName, data.rbc.advertise(clientName,
tag.topic(), tag.topic(),
tag.messageType() ); tag.messageType() );
} }
}
// Publish Json message. // Publish Json message.
{
std::lock_guard<std::mutex> lk(data.rbcMutex);
data.rbc.publish(tag.topic(), *pJsonDoc.get()); data.rbc.publish(tag.topic(), *pJsonDoc.get());
}
} // while loop } // while loop
// Tidy up. // Tidy up.
{
std::lock_guard<std::mutex> lk(data.rbcMutex);
for (auto& it : clientMap) for (auto& it : clientMap)
data.rbc.removeClient(it.second); data.rbc.removeClient(it.second);
}
} }
...@@ -24,9 +24,8 @@ class TopicPublisher ...@@ -24,9 +24,8 @@ class TopicPublisher
public: public:
TopicPublisher() = delete; TopicPublisher() = delete;
TopicPublisher(CasePacker *casePacker, TopicPublisher(CasePacker &casePacker,
RosbridgeWsClient *rbc, RosbridgeWsClient &rbc);
std::mutex *rbcMutex);
~TopicPublisher(); ~TopicPublisher();
...@@ -46,7 +45,7 @@ public: ...@@ -46,7 +45,7 @@ public:
template<class T> template<class T>
void publish(const T &msg, const std::string &topic){ 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)); publish(std::move(docPtr));
} }
...@@ -54,9 +53,8 @@ private: ...@@ -54,9 +53,8 @@ private:
JsonQueue _queue; JsonQueue _queue;
std::mutex _queueMutex; std::mutex _queueMutex;
std::atomic<bool> _running; std::atomic<bool> _running;
CasePacker *_casePacker; CasePacker &_casePacker;
RosbridgeWsClient *_rbc; RosbridgeWsClient &_rbc;
std::mutex *_rbcMutex;
CondVar _cv; CondVar _cv;
ThreadPtr _pThread; ThreadPtr _pThread;
}; };
......
#include "TopicSubscriber.h" #include "TopicSubscriber.h"
ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(ROSBridge::CasePacker *casePacker, ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
RosbridgeWsClient *rbc, std::mutex *rbcMutex) : RosbridgeWsClient &rbc) :
_casePacker(casePacker) _casePacker(casePacker)
, _rbc(rbc) , _rbc(rbc)
, _rbcMutex(rbcMutex)
, _running(false) , _running(false)
{ {
...@@ -26,14 +25,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::reset() ...@@ -26,14 +25,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::reset()
if ( _running ){ if ( _running ){
_running = false; _running = false;
{ {
std::lock_guard<std::mutex> lk(*_rbcMutex);
for (std::string clientName : _clientList){ for (std::string clientName : _clientList){
_rbc->removeClient(clientName); _rbc.removeClient(clientName);
} }
} }
{ {
std::lock_guard<std::mutex> lk(_callbackMap.mutex); std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
_callbackMap.map.clear(); _callbackMapStruct.map.clear();
} }
_clientList.clear(); _clientList.clear();
} }
...@@ -52,8 +50,8 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( ...@@ -52,8 +50,8 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
HashType hash = getHash(clientName); HashType hash = getHash(clientName);
{ {
std::lock_guard<std::mutex> lk(_callbackMap.mutex); std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
auto ret = _callbackMap.map.insert(std::make_pair(hash, callback)); // auto ret = _callbackMapStruct.map.insert(std::make_pair(hash, callback)); //
if ( !ret.second ) if ( !ret.second )
return false; // Topic subscription already present. return false; // Topic subscription already present.
...@@ -62,7 +60,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( ...@@ -62,7 +60,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
using namespace std::placeholders; using namespace std::placeholders;
auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback, auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
hash, hash,
std::ref(_callbackMap), std::ref(_callbackMapStruct),
_1, _2); _1, _2);
// std::cout << std::endl; // std::cout << std::endl;
...@@ -70,11 +68,20 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( ...@@ -70,11 +68,20 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
// std::cout << "client name: " << clientName << std::endl; // std::cout << "client name: " << clientName << std::endl;
// std::cout << "topic: " << topic << std::endl; // std::cout << "topic: " << topic << std::endl;
{ {
std::lock_guard<std::mutex> lk(*_rbcMutex); auto start = std::chrono::high_resolution_clock::now();
_rbc->addClient(clientName); _rbc.addClient(clientName);
_rbc->subscribe(clientName, auto end = std::chrono::high_resolution_clock::now();
std::cout << "add client time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
_rbc.subscribe(clientName,
topic, topic,
f ); f );
end = std::chrono::high_resolution_clock::now();
std::cout << "subscribe time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms" << std::endl;
} }
return true; return true;
...@@ -84,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( ...@@ -84,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>; using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
void ROSBridge::ComPrivate::subscriberCallback( void ROSBridge::ComPrivate::subscriberCallback(
const HashType &hash, const HashType &hash,
ROSBridge::ComPrivate::CallbackMapWrapper &mapWrapper, ROSBridge::ComPrivate::MapStruct &mapWrapper,
std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message) std::shared_ptr<WsClient::InMessage> in_message)
{ {
......
...@@ -10,7 +10,7 @@ namespace ComPrivate { ...@@ -10,7 +10,7 @@ namespace ComPrivate {
typedef std::function<void(JsonDocUPtr)> CallbackType; typedef std::function<void(JsonDocUPtr)> CallbackType;
struct CallbackMapWrapper{ struct MapStruct{
typedef std::map<HashType, CallbackType> Map; typedef std::map<HashType, CallbackType> Map;
Map map; Map map;
std::mutex mutex; std::mutex mutex;
...@@ -22,7 +22,7 @@ class TopicSubscriber ...@@ -22,7 +22,7 @@ class TopicSubscriber
public: public:
TopicSubscriber() = delete; TopicSubscriber() = delete;
TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc, std::mutex *rbcMutex); TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc);
~TopicSubscriber(); ~TopicSubscriber();
//! @brief Starts the subscriber. //! @brief Starts the subscriber.
...@@ -40,16 +40,15 @@ private: ...@@ -40,16 +40,15 @@ private:
CasePacker *_casePacker; CasePacker &_casePacker;
RosbridgeWsClient *_rbc; RosbridgeWsClient &_rbc;
std::mutex *_rbcMutex; MapStruct _callbackMapStruct;
CallbackMapWrapper _callbackMap;
ClientList _clientList; ClientList _clientList;
bool _running; bool _running;
}; };
void subscriberCallback(const HashType &hash, void subscriberCallback(const HashType &hash,
CallbackMapWrapper &mapWrapper, MapStruct &mapWrapper,
std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message); std::shared_ptr<WsClient::InMessage> in_message);
} // namespace ComPrivate } // namespace ComPrivate
......
...@@ -3,8 +3,8 @@ ...@@ -3,8 +3,8 @@
ROSBridge::ROSBridge::ROSBridge() : ROSBridge::ROSBridge::ROSBridge() :
_casePacker(&_typeFactory, &_jsonFactory) _casePacker(&_typeFactory, &_jsonFactory)
, _rbc("localhost:9090") , _rbc("localhost:9090")
, _topicPublisher(&_casePacker, &_rbc, &_rbcMutex) , _topicPublisher(_casePacker, _rbc)
, _topicSubscriber(&_casePacker, &_rbc, &_rbcMutex) , _topicSubscriber(_casePacker, _rbc)
{ {
} }
...@@ -38,7 +38,6 @@ void ROSBridge::ROSBridge::reset() ...@@ -38,7 +38,6 @@ void ROSBridge::ROSBridge::reset()
bool ROSBridge::ROSBridge::connected() bool ROSBridge::ROSBridge::connected()
{ {
std::lock_guard<std::mutex> lk(_rbcMutex);
return _rbc.connected(); return _rbc.connected();
} }
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment