Skip to content

Q
qgroundcontrol
Commit 96d3005b authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

123

parent 07a48251
Hide whitespace changes
Inline Side-by-side
Showing with 407 additions and 309 deletions
src/Snake/snake.cpp
View file @ 96d3005b
......@@ -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<int64_t> 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<double> 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<double> &graph)
{
#ifdef SHOW_TIME
......
src/Snake/snake.h
View file @ 96d3005b
......@@ -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<GeoPoint3D> &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<int8_t> &progress) {_progress = progress;}
void setScenario(Scenario &scenario) {_scenario = scenario;}
void setProgress(vector<int> &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<int64_t> 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<double> &graph);
Scenario _scenario;
BoostLineString _waypointsENU;
GeoPoint2DList _waypoints;
vector<BoostLineString> _transects;
vector<int8_t> _progress;
vector<int> _progress;
vector<uint64_t> _arrivalPathIdx;
vector<uint64_t> _returnPathIdx;
......
src/Wima/Geometry/GeoPoint3D.cpp
View file @ 96d3005b
#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());
......
src/Wima/Geometry/GeoPoint3D.h
View file @ 96d3005b
......@@ -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);
};
......
src/Wima/Snake/SnakeWorker.cc
View file @ 96d3005b
......@@ -2,58 +2,84 @@
#include <QGeoCoordinate>
#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<int> &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 &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();
}
src/Wima/Snake/SnakeWorker.h
View file @ 96d3005b
......@@ -4,23 +4,27 @@
#include <QVariant>
#include <QThread>
#include <QVector>
#include <QGeoCoordinate>
#include <vector>
#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<QVariant> QVariantList;
typedef struct Result{
QVariantList waypoints;
QVector<QGeoCoordinate> waypoints;
SnakeTiles tiles;
SnakeTilesLocal tilesLocal;
QVariantList tileCenterPoints;
QGeoCoordinate origin;
QVector<unsigned long> arrivalPathIdx;
QVector<unsigned long> 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<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:
void run() override;
private:
Scenario _scenario;
vector<int8_t> _progress;
double _lineDistance;
double _minTransectLength;
WorkerResult_t _result;
bool precondition() const;
std::vector<QGeoCoordinate> _mArea;
std::vector<QGeoCoordinate> _sArea;
std::vector<QGeoCoordinate> _corridor;
std::vector<int> _progress;
double _lineDistance;
double _minTransectLength;
double _tileWidth;
double _tileHeight;
double _minTileArea;
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);
}
}
src/Wima/WaypointManager/Slicer.cpp
View file @ 96d3005b
......@@ -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;
......
src/Wima/WimaController.cc
View file @ 96d3005b
......@@ -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<QGeoCoordinate>()};
mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
mArea.type = AreaType::MeasurementArea;
Area sArea;
for (auto variant : _serviceArea.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
sArea.type = AreaType::ServiceArea;
Area corridor;
for (auto variant : _corridor.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
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 &centerPoint = 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<std::chrono::milliseconds>(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<QGeoCoordinate>());
_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.
......
src/Wima/WimaController.h
View file @ 96d3005b
......@@ -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<WaypointManager::ManagerBase*>;
ManagerList _managerList;
// Settings Facts.
QMap<QString, FactMetaData*> _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;
......
src/comm/ros_bridge/include/ROSBridge.h
View file @ 96d3005b
......@@ -45,7 +45,6 @@ private:
JsonFactory _jsonFactory;
CasePacker _casePacker;
RosbridgeWsClient _rbc;
std::mutex _rbcMutex;
ComPrivate::TopicPublisher _topicPublisher;
ComPrivate::TopicSubscriber _topicSubscriber;
......
src/comm/ros_bridge/include/RosBridgeClient.h
View file @ 96d3005b
......@@ -17,6 +17,7 @@
#include <thread>
#include <future>
#include <functional>
#include <mutex>
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
......@@ -25,6 +26,7 @@ class RosbridgeWsClient
{
std::string server_port_path;
std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
std::mutex map_mutex;
// Starts the client.
void start(const std::string& client_name, std::shared_ptr<WsClient> 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<std::mutex> 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<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it == client_map.end())
{
......@@ -151,6 +154,8 @@ public:
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);
if (it != client_map.end())
{
......@@ -161,6 +166,8 @@ public:
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);
if (it != client_map.end())
{
......@@ -183,12 +190,22 @@ public:
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);
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<WsClient> 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<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::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<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::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<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
......
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @ 96d3005b
......@@ -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<bool> &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<std::mutex> lk(data.rbcMutex);
// data.rbc.addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
// data.rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
// }
// Main Loop.
while(data.running.load()){
std::unique_lock<std::mutex> 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<std::mutex> 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<std::mutex> lk(data.rbcMutex);
data.rbc.publish(tag.topic(), *pJsonDoc.get());
}
data.rbc.publish(tag.topic(), *pJsonDoc.get());
} // while loop
// Tidy up.
{
std::lock_guard<std::mutex> lk(data.rbcMutex);
for (auto& it : clientMap)
data.rbc.removeClient(it.second);
}
for (auto& it : clientMap)
data.rbc.removeClient(it.second);
}
src/comm/ros_bridge/include/TopicPublisher.h
View file @ 96d3005b
......@@ -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<class T>
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<bool> _running;
CasePacker *_casePacker;
RosbridgeWsClient *_rbc;
std::mutex *_rbcMutex;
CasePacker &_casePacker;
RosbridgeWsClient &_rbc;
CondVar _cv;
ThreadPtr _pThread;
};
......
src/comm/ros_bridge/include/TopicSubscriber.cpp
View file @ 96d3005b
#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<std::mutex> lk(*_rbcMutex);
for (std::string clientName : _clientList){
_rbc->removeClient(clientName);
_rbc.removeClient(clientName);
}
}
{
std::lock_guard<std::mutex> lk(_callbackMap.mutex);
_callbackMap.map.clear();
std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
_callbackMapStruct.map.clear();
}
_clientList.clear();
}
......@@ -52,8 +50,8 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
HashType hash = getHash(clientName);
{
std::lock_guard<std::mutex> lk(_callbackMap.mutex);
auto ret = _callbackMap.map.insert(std::make_pair(hash, callback)); //
std::lock_guard<std::mutex> 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<std::mutex> 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<std::chrono::milliseconds>(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<std::chrono::milliseconds>(end-start).count()
<< " ms" << std::endl;
}
return true;
......@@ -84,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
void ROSBridge::ComPrivate::subscriberCallback(
const HashType &hash,
ROSBridge::ComPrivate::CallbackMapWrapper &mapWrapper,
ROSBridge::ComPrivate::MapStruct &mapWrapper,
std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message)
{
......
src/comm/ros_bridge/include/TopicSubscriber.h
View file @ 96d3005b
......@@ -10,7 +10,7 @@ namespace ComPrivate {
typedef std::function<void(JsonDocUPtr)> CallbackType;
struct CallbackMapWrapper{
struct MapStruct{
typedef std::map<HashType, CallbackType> 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<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message);
} // namespace ComPrivate
......
src/comm/ros_bridge/src/ROSBridge.cpp
View file @ 96d3005b
......@@ -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<std::mutex> lk(_rbcMutex);
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