Commit a0ede0e6 authored by Valentin Platzgummer's avatar Valentin Platzgummer
parents 4bb718fe 96d3005b
......@@ -27,6 +27,8 @@ QGCROOT = $$PWD
DebugBuild {
DESTDIR = $${OUT_PWD}/debug
#DEFINES += DEBUG
#DEFINES += ROS_BRIDGE_CLIENT_DEBUG
}
else {
DESTDIR = $${OUT_PWD}/release
......
......@@ -251,7 +251,8 @@ FlightMap {
delegate: MapCircle{
center: modelData
border.color: "transparent"
border.color: "black"
border.width: 1
color: getColor(wimaController.nemoProgress[index])
radius: 0.6
opacity: 1
......
......@@ -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
......
......@@ -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;
......
#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());
......
......@@ -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);
};
......
......@@ -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();
}
......@@ -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);
}
}
......@@ -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;
......
This diff is collapsed.
......@@ -46,7 +46,7 @@
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 5000 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
using namespace snake;
......@@ -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.
......@@ -351,16 +348,12 @@ private slots:
private:
using StatusMap = std::map<int, QString>;
// Snake.
void _progressFromJson (JsonDocUPtr pDoc,
QNemoProgress &progress);
// Controllers.
PlanMasterController *_masterController;
MissionController *_missionController;
// Wima Data.
QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area
......@@ -375,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;
......@@ -406,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;
......@@ -418,6 +410,7 @@ private:
QNemoHeartbeat _nemoHeartbeat; // measurement progress
int _fallbackStatus;
ROSBridgePtr _pRosBridge;
bool _bridgeSetupDone;
static StatusMap _nemoStatusMap;
// Periodic tasks.
......
......@@ -5,7 +5,7 @@
#include <memory>
#include <deque>
#include <set>
#include <unordered_map>
namespace ROSBridge {
namespace ComPrivate {
......@@ -15,7 +15,8 @@ typedef rapidjson::Document JsonDoc;
typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
typedef std::deque<JsonDocUPtr> JsonQueue;
typedef std::size_t HashType;
typedef std::set<HashType> HashSet;
using ClientMap = std::unordered_map<HashType, std::string>;
static const char* _topicAdvertiserKey = "topic_advertiser";
static const char* _topicPublisherKey = "topic_publisher";
......
......@@ -79,6 +79,17 @@ struct GeometryMsgs {
};
};
struct GeographicMsgs {
static StringType label() {return "geographic_msgs";}
//!
//! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
struct GeoPointGroup {
static StringType label() {return "GeoPoint";}
};
};
struct JSKRecognitionMsgs {