Skip to content

Q
qgroundcontrol
Commit 0510d157 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

snake working, gui still stuttering sometimes

parent 50929c20
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 3112 additions and 1599 deletions
Paths/KlingenbachGross.wima 0 → 100644
View file @ 0510d157
This diff is collapsed.
Paths/KlingenbachKlein.wima 0 → 100644
View file @ 0510d157
{
"AreaItems": [
{
"AreaType": "Measurement Area",
"BorderPolygonOffset": 6,
"BottomLayerAltitude": 5,
"LayerDistance": 1,
"NumberOfLayers": 1,
"ShowBorderPolygon": 0,
"maxAltitude": 30,
"polygon": [
[
47.767875513636454,
16.53043285498356
],
[
47.767996632271384,
16.530579999131646
],
[
47.76784223516915,
16.53070246118625
],
[
47.76777418124312,
16.530481637145055
]
]
},
{
"AreaType": "Service Area",
"BorderPolygonOffset": 6,
"ShowBorderPolygon": 0,
"maxAltitude": 30,
"polygon": [
[
47.76777317830783,
16.530413969584316
],
[
47.76785089069791,
16.530667046992818
],
[
47.76775614544286,
16.5307494063876
],
[
47.76766140001518,
16.530447230100037
]
]
}
],
"MissionItems": {
"fileType": "Plan",
"geoFence": {
"circles": [
],
"polygons": [
],
"version": 2
},
"groundStation": "QGroundControl",
"mission": {
"cruiseSpeed": 15,
"firmwareType": 3,
"hoverSpeed": 1,
"items": [
{
"autoContinue": true,
"command": 22,
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
null,
47.76776040370392,
16.530569413178814,
8
],
"type": "SimpleItem"
},
{
"TransectStyleComplexItem": {
"CameraCalc": {
"AdjustedFootprintFrontal": 25,
"AdjustedFootprintSide": 25,
"CameraName": "Manual (no camera specs)",
"DistanceToSurface": 15,
"DistanceToSurfaceRelative": true,
"version": 1
},
"CameraShots": 0,
"CameraTriggerInTurnAround": true,
"FollowTerrain": false,
"HoverAndCapture": false,
"Items": [
{
"autoContinue": true,
"command": 16,
"doJumpId": 2,
"frame": 3,
"params": [
0,
0,
0,
null,
47.767881573082065,
16.530440216459883,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 3,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76787098888673,
16.530563032107995,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 4,
"frame": 3,
"params": [
0,
0,
0,
null,
47.7678626911699,
16.530686236245202,
15
],
"type": "SimpleItem"
}
],
"Refly90Degrees": false,
"TurnAroundDistance": 10,
"VisualTransectPoints": [
[
47.767881573082065,
16.530440216459883
],
[
47.76787098888673,
16.530563032107995
],
[
47.7678626911699,
16.530686236245202
]
],
"version": 1
},
"complexItemType": "circularSurvey",
"deltaAlpha": 3,
"deltaR": 5,
"fixedDirection": false,
"polygon": [
[
47.767875513636454,
16.53043285498356
],
[
47.767996632271384,
16.530579999131646
],
[
47.76784223516915,
16.53070246118625
],
[
47.76777418124312,
16.530481637145055
]
],
"referencePointAlt": 0,
"referencePointLat": 47.77086091347008,
"referencePointLong": 16.531071041719485,
"reverse": false,
"transectMinLength": 15,
"type": "ComplexItem",
"version": 1
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 0,
"AltitudeMode": 1,
"autoContinue": true,
"command": 21,
"doJumpId": 8,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76776040370392,
16.530569413178814,
0
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
47.76776040370392,
16.530569413178814,
178
],
"vehicleType": 2,
"version": 2
},
"rallyPoints": {
"points": [
],
"version": 2
},
"version": 1
}
}
qgroundcontrol.pro
View file @ 0510d157
......@@ -29,6 +29,7 @@ DebugBuild {
DESTDIR = $${OUT_PWD}/debug
DEFINES += DEBUG
DEFINES += SNAKE_SHOW_TIME
DEFINES += SNAKE_DEBUG
#DEFINES += ROS_BRIDGE_DEBUG
}
else {
......@@ -366,7 +367,7 @@ INCLUDEPATH += \
src/QtLocationPlugin \
src/QtLocationPlugin/QMLControl \
src/Settings \
src/Snake \
src/Wima/Snake \
src/Terrain \
src/VehicleSetup \
src/ViewWidgets \
......@@ -410,29 +411,28 @@ FORMS += \
#
HEADERS += \
src/Snake/clipper/clipper.hpp \
src/Snake/mapbox/feature.hpp \
src/Snake/mapbox/geometry.hpp \
src/Snake/mapbox/geometry/box.hpp \
src/Snake/mapbox/geometry/empty.hpp \
src/Snake/mapbox/geometry/envelope.hpp \
src/Snake/mapbox/geometry/for_each_point.hpp \
src/Snake/mapbox/geometry/geometry.hpp \
src/Snake/mapbox/geometry/line_string.hpp \
src/Snake/mapbox/geometry/multi_line_string.hpp \
src/Snake/mapbox/geometry/multi_point.hpp \
src/Snake/mapbox/geometry/multi_polygon.hpp \
src/Snake/mapbox/geometry/point.hpp \
src/Snake/mapbox/geometry/point_arithmetic.hpp \
src/Snake/mapbox/geometry/polygon.hpp \
src/Snake/mapbox/geometry_io.hpp \
src/Snake/mapbox/optional.hpp \
src/Snake/mapbox/polylabel.hpp \
src/Snake/mapbox/recursive_wrapper.hpp \
src/Snake/mapbox/variant.hpp \
src/Snake/mapbox/variant_io.hpp \
src/Snake/snake.h \
src/Snake/snake_typedefs.h \
src/Wima/Snake/clipper/clipper.hpp \
src/Wima/Snake/mapbox/feature.hpp \
src/Wima/Snake/mapbox/geometry.hpp \
src/Wima/Snake/mapbox/geometry/box.hpp \
src/Wima/Snake/mapbox/geometry/empty.hpp \
src/Wima/Snake/mapbox/geometry/envelope.hpp \
src/Wima/Snake/mapbox/geometry/for_each_point.hpp \
src/Wima/Snake/mapbox/geometry/geometry.hpp \
src/Wima/Snake/mapbox/geometry/line_string.hpp \
src/Wima/Snake/mapbox/geometry/multi_line_string.hpp \
src/Wima/Snake/mapbox/geometry/multi_point.hpp \
src/Wima/Snake/mapbox/geometry/multi_polygon.hpp \
src/Wima/Snake/mapbox/geometry/point.hpp \
src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp \
src/Wima/Snake/mapbox/geometry/polygon.hpp \
src/Wima/Snake/mapbox/geometry_io.hpp \
src/Wima/Snake/mapbox/optional.hpp \
src/Wima/Snake/mapbox/polylabel.hpp \
src/Wima/Snake/mapbox/recursive_wrapper.hpp \
src/Wima/Snake/mapbox/variant.hpp \
src/Wima/Snake/mapbox/variant_io.hpp \
src/Wima/Snake/snake.h \
src/Wima/Geometry/GenericPolygon.h \
src/Wima/Geometry/GenericPolygonArray.h \
src/Wima/Geometry/GeoPoint3D.h \
......@@ -500,8 +500,8 @@ HEADERS += \
src/comm/ros_bridge/include/topic_subscriber.h \
src/comm/utilities.h
SOURCES += \
src/Snake/clipper/clipper.cpp \
src/Snake/snake.cpp \
src/Wima/Snake/clipper/clipper.cpp \
src/Wima/Snake/snake.cpp \
src/Wima/Geometry/GeoPoint3D.cpp \
src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \
......
src/FlightDisplay/FlightDisplayViewMap.qml
View file @ 0510d157
......@@ -243,10 +243,8 @@ FlightMap {
// Add Snake tiles center points to the map
MapItemView {
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
model: _enable ? wimaController.snakeTileCenterPoints : 0
delegate: MapCircle{
......@@ -260,7 +258,7 @@ FlightMap {
function getColor(progress) {
if (progress < 50)
return "red"
return "orangered"
if (progress < 100)
return "orange"
......@@ -271,10 +269,8 @@ FlightMap {
// Add Snake tiles to the map
MapItemView {
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
model: _enable ? wimaController.snakeTiles : 0
delegate: MapPolygon{
......
src/FlightDisplay/FlightDisplayWimaMenu.qml
View file @ 0510d157
......@@ -98,6 +98,7 @@ Item {
property var enableWimaFact: wimaController.enableWimaController
property bool enableWimaBoolean: enableWimaFact.value
property var enableSnakeFact: wimaController.enableSnake
onAccept: {
if (enableWimaBoolean) {
......@@ -107,6 +108,7 @@ Item {
} else {
enableWimaFact.value = true
enableWimaMouseArea.enabled = false
enableSnakeCheckBox.checkedState = false;
timer.stop()
}
}
......@@ -184,6 +186,7 @@ Item {
spacing: ScreenTools.defaultFontPixelHeight * 0.5
FactCheckBox {
id: enableSnakeCheckBox
text: wimaController.enableSnake.value ?
qsTr("Disable Snake")
: qsTr("Enable Snake")
......
src/FlightMap/MapItems/WimaPlanMapItems.qml
View file @ 0510d157
......@@ -30,29 +30,8 @@ Item {
property var _map: map
property var _missionLineViewComponent
property var zoom: map.zoomLevel
property bool showItems: true
// onZoomChanged: {
// console.log('zoomLevel')
// console.log(zoom)
// }
function hideNumber() {
if (zoom > 19.5) {
return 1
} else if (zoom > 19) {
return 2
} else if (zoom > 17) {
return 4
} else if (zoom > 14) {
return 8
} else {
return -1
}
}
Component {
id: missionLineViewComponent
......@@ -64,7 +43,6 @@ Item {
}
}
Repeater {
model: largeMapView ? ( showItems ? missionItems : 0) : 0
......@@ -72,22 +50,10 @@ Item {
map: _map
color: _root.color
zOrder: _root.zOrderWP
visible: isVisible(index) && _root.visible
function isVisible(index) {
var num = hideNumber()
if (num > 0) {
return ((index+num) % num) == 0 ? true : false
} else {
return false
}
}
visible: _root.visible
}
}
Component.onCompleted: {
_missionLineViewComponent = missionLineViewComponent.createObject(map)
if (_missionLineViewComponent.status === Component.Error)
......@@ -98,6 +64,4 @@ Item {
Component.onDestruction: {
_missionLineViewComponent.destroy()
}
}
src/Snake/snake_typedefs.h deleted 100644 → 0
View file @ 50929c20
#pragma once
#include <vector>
#include <array>
#include <boost/geometry.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/angle/degrees.hpp>
namespace bg = boost::geometry;
namespace bu = boost::units;
namespace snake {
typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
//typedef std::vector<BoostPoint> BoostPointList;
typedef bg::model::polygon<BoostPoint> BoostPolygon;
typedef bg::model::linestring<BoostPoint> BoostLineString;
typedef std::vector<std::vector<int64_t>> Int64Matrix;
typedef bg::model::box<BoostPoint> BoostBox;
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::plane_angle> Angle;
}
namespace boost{
namespace geometry{
namespace model {
bool operator==(snake::BoostPoint p1, snake::BoostPoint p2){
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2){
return !(p1 == p2);
}
} // namespace model
} // namespace geometry
} // namespace boost
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ 0510d157
This diff is collapsed.
src/Wima/Geometry/WimaMeasurementArea.h
View file @ 0510d157
......@@ -7,66 +7,53 @@
#include "SettingsFact.h"
class WimaMeasurementArea : public WimaArea
{
Q_OBJECT
class WimaMeasurementArea : public WimaArea {
Q_OBJECT
public:
WimaMeasurementArea(QObject* parent = nullptr);
WimaMeasurementArea(const WimaMeasurementArea &other, QObject *parent = nullptr);
WimaMeasurementArea &operator=(const WimaMeasurementArea &other);
Q_PROPERTY(Fact* bottomLayerAltitude READ bottomLayerAltitudeFact CONSTANT)
Q_PROPERTY(Fact* numberOfLayers READ numberOfLayersFact CONSTANT)
Q_PROPERTY(Fact* layerDistance READ layerDistanceFact CONSTANT)
// Overrides from WimaPolygon
QString mapVisualQML (void) const { return "WimaMeasurementAreaMapVisual.qml";}
QString editorQML (void) const { return "WimaMeasurementAreaEditor.qml";}
// Property accessors
Fact* bottomLayerAltitudeFact (void) { return &_bottomLayerAltitude;}
Fact* numberOfLayersFact (void) { return &_numberOfLayers;}
Fact* layerDistanceFact (void) { return &_layerDistance;}
double bottomLayerAltitude (void) const { return _bottomLayerAltitude.rawValue().toDouble();}
int numberOfLayers (void) const { return _numberOfLayers.rawValue().toInt();}
double layerDistance (void) const { return _layerDistance.rawValue().toDouble();}
// Member Methodes
void saveToJson(QJsonObject& json);
bool loadFromJson(const QJsonObject& json, QString &errorString);
// Friends
friend void print(const WimaMeasurementArea& area, QString outputStr);
friend void print(const WimaMeasurementArea& area);
// Static Variables
static const char* settingsGroup;
static const char* bottomLayerAltitudeName;
static const char* numberOfLayersName;
static const char* layerDistanceName;
static const char* WimaMeasurementAreaName;
WimaMeasurementArea(QObject *parent = nullptr);
WimaMeasurementArea(const WimaMeasurementArea &other,
QObject *parent = nullptr);
WimaMeasurementArea &operator=(const WimaMeasurementArea &other);
Q_PROPERTY(Fact *borderPolygonOffset READ borderPolygonOffsetFact CONSTANT)
// Overrides from WimaPolygon
QString mapVisualQML(void) const {
return "WimaMeasurementAreaMapVisual.qml";
}
QString editorQML(void) const { return "WimaMeasurementAreaEditor.qml"; }
// Member Methodes
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// Friends
friend void print(const WimaMeasurementArea &area, QString outputStr);
friend void print(const WimaMeasurementArea &area);
// Static Variables
static const char *settingsGroup;
static const char *tileHeightName;
static const char *tileWidthName;
static const char *minTileAreaName;
static const char *transcetDistanceName;
static const char *minTransectLength;
static const char *WimaMeasurementAreaName;
signals:
void bottomLayerAltitudeChanged (void);
void numberOfLayersChanged (void);
void layerDistanceChanged (void);
public slots:
void setBottomLayerAltitude (double altitude);
void setNumberOfLayers (double numLayers);
void setLayerDistance (double layerDistance);
private:
// Member Methodes
void init();
// Member Methodes
void init();
// Members
QMap<QString, FactMetaData*> _metaDataMap;
// Members
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _bottomLayerAltitude;
SettingsFact _numberOfLayers;
SettingsFact _layerDistance;
SettingsFact _tileWidth;
SettingsFact _tileHeight;
SettingsFact _minTileArea;
SettingsFact _transectDistance;
SettingsFact _minTransectLength;
};
src/Wima/Snake/SnakeDataManager_old.cc deleted 100644 → 0
View file @ 50929c20
This diff is collapsed.
src/Wima/Snake/SnakeThread.cc
View file @ 0510d157
......@@ -39,7 +39,7 @@ public:
Input()
: tileWidth(5 * si::meter), tileHeight(5 * si::meter),
minTileArea(1 * si::meter * si::meter), lineDistance(2 * si::meter),
minTileArea(5 * si::meter * si::meter), lineDistance(2 * si::meter),
minTransectLength(1 * si::meter), scenarioChanged(true),
progressChanged(true), routeParametersChanged(true) {}
......@@ -490,54 +490,55 @@ void SnakeThread::run() {
this->pImpl->output.returnPathENU.clear();
this->pImpl->output.waypoints.clear();
this->pImpl->output.waypointsENU.clear();
// // Store arrival path.
// const auto &firstWaypoint = transectsRouted.front().front();
// long startIdx = 0;
// for (long i = 0; i < long(route.size()); ++i) {
// const auto &boostVertex = route[i];
// if (boostVertex == firstWaypoint) {
// startIdx = i;
// break;
// }
// QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, boostVertex, geoVertex);
// this->pImpl->output.arrivalPathENU.push_back(enuVertex);
// this->pImpl->output.arrivalPath.push_back(geoVertex);
// }
// // Store return path.
// long endIdx = 0;
// const auto &lastWaypoint = transectsRouted.back().back();
// for (long i = route.size() - 1; i >= 0; --i) {
// const auto &boostVertex = route[i];
// if (boostVertex == lastWaypoint) {
// endIdx = i;
// break;
// }
// QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, boostVertex, geoVertex);
// this->pImpl->output.returnPathENU.push_back(enuVertex);
// this->pImpl->output.returnPath.push_back(geoVertex);
// }
// // Store waypoints.
// for (long i = startIdx; i <= endIdx; ++i) {
// const auto &boostVertex = route[i];
// QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, boostVertex, geoVertex);
// this->pImpl->output.waypointsENU.push_back(enuVertex);
// this->pImpl->output.waypoints.push_back(geoVertex);
// }
for (const auto &t : transects) {
for (const auto &v : t) {
QPointF enuVertex{v.get<0>(), v.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, v, geoVertex);
this->pImpl->output.waypointsENU.push_back(enuVertex);
this->pImpl->output.waypoints.push_back(geoVertex);
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
long startIdx = 0;
for (long i = 0; i < long(route.size()); ++i) {
const auto &boostVertex = route[i];
if (boostVertex == firstWaypoint) {
startIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->output.arrivalPathENU.push_back(enuVertex);
this->pImpl->output.arrivalPath.push_back(geoVertex);
}
// Store return path.
long endIdx = 0;
const auto &lastWaypoint = transectsRouted.back().back();
for (long i = route.size() - 1; i >= 0; --i) {
const auto &boostVertex = route[i];
if (boostVertex == lastWaypoint) {
endIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->output.returnPathENU.push_back(enuVertex);
this->pImpl->output.returnPath.push_back(geoVertex);
}
// Store waypoints.
for (long i = startIdx; i <= endIdx; ++i) {
const auto &boostVertex = route[i];
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->output.waypointsENU.push_back(enuVertex);
this->pImpl->output.waypoints.push_back(geoVertex);
}
// Store waypoints.
// for (const auto &t : transects) {
// for (const auto &v : t) {
// QPointF enuVertex{v.get<0>(), v.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, v, geoVertex);
// this->pImpl->output.waypointsENU.push_back(enuVertex);
// this->pImpl->output.waypoints.push_back(geoVertex);
// }
// }
this->pImpl->output.waypointsUpdated = true;
}
}
src/Wima/Snake/route.cpp 0 → 100644
View file @ 0510d157
bool flight_plan::route(const BoostPolygon &area,
const flight_plan::Transects &transects,
Transects &transectsRouted, flight_plan::Route &route,
string &errorString) {
//=======================================
// Route Transects using Google or-tools.
//=======================================
// Create vertex list;
BoostLineString vertices;
size_t n0 = 0;
for (const auto &t : transects) {
n0 += std::min<std::size_t>(t.size(), 2);
}
vertices.reserve(n0);
struct TransectInfo {
TransectInfo(size_t n, bool f) : index(n), front(f) {}
size_t index;
bool front;
};
std::vector<TransectInfo> transectInfoList;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
vertices.push_back(t.front());
transectInfoList.push_back(TransectInfo{i, true});
if (t.size() >= 2) {
vertices.push_back(t.back());
transectInfoList.push_back(TransectInfo{i, false});
}
}
for (long i = 0; i < long(area.outer().size()) - 1; ++i) {
vertices.push_back(area.outer()[i]);
}
for (auto &ring : area.inners()) {
for (auto &vertex : ring)
vertices.push_back(vertex);
}
size_t n1 = vertices.size();
// Generate routing model.
RoutingDataModel dataModel;
Matrix<double> connectionGraph(n1, n1);
// Offset joined area.
BoostPolygon areaOffset;
offsetPolygon(area, areaOffset, detail::offsetConstant);
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms"
<< endl;
#endif
// Create Routing Index Manager.
RoutingIndexManager manager(dataModel.distanceMatrix.getN(),
dataModel.numVehicles, dataModel.depot);
// Create Routing Model.
RoutingModel routing(manager);
// Create and register a transit callback.
const int transitCallbackIndex = routing.RegisterTransitCallback(
[&dataModel, &manager](int64 from_index, int64 to_index) -> int64 {
// Convert from routing variable Index to distance matrix NodeIndex.
auto from_node = manager.IndexToNode(from_index).value();
auto to_node = manager.IndexToNode(to_index).value();
return dataModel.distanceMatrix.get(from_node, to_node);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
// Define Constraints (this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
#ifdef SNAKE_DEBUG
std::cout << "snake::route(): Constraints:" << std::endl;
#endif
Solver *solver = routing.solver();
size_t index = 0;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
#ifdef SNAKE_DEBUG
std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl;
#endif
if (t.size() >= 2) {
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index));
auto idx1 =
manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1));
auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
auto c = solver->MakeNonEquality(cond0, cond1);
solver->AddConstraint(c);
#ifdef SNAKE_DEBUG
std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next("
<< index + 1 << ") == " << index << " )" << std::endl;
#endif
index += 2;
} else {
index += 1;
}
}
// Setting first solution heuristic.
RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
google::protobuf::Duration *tMax =
new google::protobuf::Duration(); // seconds
tMax->set_seconds(10);
searchParameters.set_allocated_time_limit(tMax);
// Solve the problem.
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
const Assignment *solution = routing.SolveWithParameters(searchParameters);
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time routing.SolveWithParameters(): " << delta.count()
<< " ms" << endl;
#endif
if (!solution || solution->Size() <= 1) {
errorString = "Not able to solve the routing problem.";
return false;
}
// Extract index list from solution.
index = routing.Start(0);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)) {
index = solution->Value(routing.NextVar(index));
route_idx.push_back(manager.IndexToNode(index).value());
}
// Helper Lambda.
auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
std::vector<BoostPoint> &path) {
for (auto idx : idxArray)
path.push_back(vertices[idx]);
};
// Construct route.
for (size_t i = 0; i < route_idx.size() - 1; ++i) {
size_t idx0 = route_idx[i];
size_t idx1 = route_idx[i + 1];
const auto &info1 = transectInfoList[idx0];
const auto &info2 = transectInfoList[idx1];
if (info1.index == info2.index) { // same transect?
if (!info1.front) { // transect reversal needed?
BoostLineString reversedTransect;
const auto &t = transects[info1.index];
for (auto it = t.end() - 1; it >= t.begin(); --it) {
reversedTransect.push_back(*it);
}
transectsRouted.push_back(reversedTransect);
for (auto it = reversedTransect.begin();
it < reversedTransect.end() - 1; ++it) {
route.push_back(*it);
}
} else {
const auto &t = transects[info1.index];
for (auto it = t.begin(); it < t.end() - 1; ++it) {
route.push_back(*it);
}
transectsRouted.push_back(t);
}
} else {
std::vector<size_t> idxList;
shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
if (i != route_idx.size() - 2) {
idxList.pop_back();
}
idx2Vertex(idxList, route);
}
}
return true;
}
src/Snake/snake.cpp → src/Wima/Snake/snake.cpp
View file @ 0510d157
......@@ -444,6 +444,7 @@ bool Scenario::update() {
bg::correct(_mArea);
bg::correct(_sArea);
bg::correct(_corridor);
polygonCenter(_sArea, _homePosition);
if (!_calculateJoinedArea())
return false;
if (!_calculateBoundingBox())
......@@ -965,20 +966,28 @@ bool flight_plan::route(const BoostPolygon &area,
// Define Constraints (this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
#ifdef SNAKE_DEBUG
std::cout << "snake::route(): Constraints:" << std::endl;
#endif
Solver *solver = routing.solver();
size_t index = 0;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
#ifdef SNAKE_DEBUG
std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl;
#endif
if (t.size() >= 2) {
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index));
auto idx1 =
manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1));
auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
vector<IntVar *> conds{cond0, cond1};
auto c = solver->MakeAllDifferent(conds);
solver->MakeRejectFilter();
auto c = solver->MakeNonEquality(cond0, cond1);
solver->AddConstraint(c);
#ifdef SNAKE_DEBUG
std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next("
<< index + 1 << ") == " << index << " )" << std::endl;
#endif
index += 2;
} else {
index += 1;
......
src/Snake/snake.h → src/Wima/Snake/snake.h
View file @ 0510d157
......@@ -242,7 +242,7 @@ bool route(const BoostPolygon &area, const Transects &transects,
namespace detail {
const double offsetConstant =
0.5; // meter, polygon offset to compenstate for numerical inaccurracies.
1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
} // namespace snake
......
src/Wima/Snake/snake_old.cpp 0 → 100644
View file @ 0510d157
This diff is collapsed.
src/Wima/WimaController.cc
View file @ 0510d157
......@@ -138,6 +138,11 @@ WimaController::WimaController(QObject *parent)
connect(&_enableSnake, &Fact::rawValueChanged, this,
&WimaController::_enableSnakeChangedHandler);
_enableSnakeChangedHandler();
connect(&_enableWimaController, &Fact::rawValueChanged, [this] {
if (!this->_enableWimaController.rawValue().toBool()) {
this->_enableSnake.setCookedValue(QVariant(false));
}
});
// Snake Waypoint Manager.
connect(&_enableSnake, &Fact::rawValueChanged, this,
......@@ -193,7 +198,10 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact *WimaController::altitude() { return &_altitude; }
QmlObjectListModel *WimaController::snakeTiles() { return &this->tiles; }
QmlObjectListModel *WimaController::snakeTiles() {
static QmlObjectListModel list;
return &list;
}
QVariantList WimaController::snakeTileCenterPoints() {
return _currentThread->tileCenterPoints();
......@@ -719,6 +727,9 @@ void WimaController::_initSmartRTL() {
}
void WimaController::_threadFinishedHandler() {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
if (!_snakeThread.errorMessage().isEmpty()) {
qDebug() << _snakeThread.errorMessage();
}
......@@ -735,11 +746,27 @@ void WimaController::_threadFinishedHandler() {
_nemoInterface.setTilesENU(_snakeThread.tilesENU());
_nemoInterface.setENUOrigin(_snakeThread.ENUOrigin());
}
#ifdef SNAKE_SHOW_TIME
auto end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): tiles: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
#endif
if (_snakeThread.progressUpdated()) {
emit nemoProgressChanged();
}
#ifdef SNAKE_SHOW_TIME
end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): progress: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
#endif
if (_snakeThread.waypointsUpdated()) {
// Copy waypoints to waypoint manager.
_snakeWM.clear();
......@@ -759,6 +786,14 @@ void WimaController::_threadFinishedHandler() {
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
#ifdef SNAKE_SHOW_TIME
end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): waypoints: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
#endif
}
void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
......
src/WimaView/WimaMeasurementAreaEditor.qml
View file @ 0510d157
......@@ -21,14 +21,8 @@ Rectangle {
color: qgcPal.windowShadeDark
radius: _radius
// The following properties must be available up the hierarchy chain
//property real availableWidth ///< Width for control
//property var areaItem ///< Mission Item for editor
property real _margin: ScreenTools.defaultFontPixelWidth / 2
property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5
//property var polyline: areaItem.polyline
//property bool polylineInteractive: polyline.interactive
property bool polygonInteractive: areaItem.interactive
property var polygon: areaItem
property bool initNecesarry: true
......@@ -37,9 +31,6 @@ Rectangle {
polygon.interactive = polygonInteractive;
}
QGCPalette { id: qgcPal; colorGroupEnabled: true }
Column {
......@@ -68,70 +59,77 @@ Rectangle {
rowSpacing: _margin
columns: 2
QGCLabel { text: qsTr("Altitude") }
QGCLabel { text: qsTr("Offset") }
FactTextField {
fact: areaItem.bottomLayerAltitude
fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
}
FactCheckBox {
text: qsTr("Border Polygon")
fact: areaItem.showBorderPolygon
//enabled: !missionItem.followTerrain
}
Item {
height: ScreenTools.defaultFontPixelHeight / 2
width: 1
}
} // Column - Settings
SectionHeader {
id: snakeHeader
text: qsTr("Snake")
}
Column {
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
columnSpacing: _margin
rowSpacing: _margin
columns: 2
QGCLabel { text: qsTr("Layers") }
QGCLabel { text: qsTr("Tile Height") }
FactTextField {
fact: areaItem.numberOfLayers
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Layer Dist.") }
QGCLabel { text: qsTr("Tile Width") }
FactTextField {
fact: areaItem.layerDistance
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Offset") }
QGCLabel { text: qsTr("Min. Tile Area") }
FactTextField {
fact: areaItem.borderPolygonOffset
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
}
FactCheckBox {
text: qsTr("Border Polygon")
fact: areaItem.showBorderPolygon
//enabled: !missionItem.followTerrain
}
QGCLabel { text: qsTr("Transect Distance") }
Item {
height: ScreenTools.defaultFontPixelHeight / 2
width: 1
}
} // Column - Settings
/*SectionHeader {
id: polylineHeader
text: qsTr("Gateway Poly Line")
}
FactTextField {
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
QGCButton {
id: polylineEditor
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: polylineInteractive ? "Done" : "Edit"
QGCLabel { text: qsTr("Min. Transect Length") }
onClicked: editPolyline()
}
FactTextField {
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
}
QGCButton {
id: swapEndpoints
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: "Swap End-Points"
onClicked: polyline.swapEndPoints()
}*/
} // Snake
SectionHeader {
id: statsHeader
......
src/WimaView/WimaServiceAreaEditor.qml
View file @ 0510d157
......@@ -21,41 +21,12 @@ Rectangle {
color: qgcPal.windowShadeDark
radius: _radius
// The following properties must be available up the hierarchy chain
//property real availableWidth ///< Width for control
//property var areaItem ///< Mission Item for editor
property real _margin: ScreenTools.defaultFontPixelWidth / 2
property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5
//property var polyline: areaItem.polyline
//property bool polylineInteractive: polyline.interactive
property bool polygonInteractive: areaItem.interactive
property var polygon: areaItem
property bool initNecesarry: true
/*onPolylineInteractiveChanged: {
polyline.interactive = polylineInteractive;
}*/
// onPolygonInteractiveChanged: {
// polygon.interactive = polygonInteractive;
// }
/*function editPolyline(){
if (polylineInteractive){
//polyline.interactive = false;
polylineInteractive = false;
//polygonInteractive = true;
}else{
//polyline.interactive = true;
polylineInteractive = true;
//polygonInteractive = false;
}
}*/
QGCPalette { id: qgcPal; colorGroupEnabled: true }
Column {
......@@ -90,28 +61,6 @@ Rectangle {
fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
/*QGCLabel {
text: qsTr("Bottom Layer Altitude")
}
FactTextField {
fact: areaItem.bottomLayerAltitude
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Number of Layers") }
FactTextField {
fact: areaItem.numberOfLayers
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Layer Distance") }
FactTextField {
fact: areaItem.layerDistance
Layout.fillWidth: true
}*/
}
Item {
......@@ -125,31 +74,6 @@ Rectangle {
fact: areaItem.showBorderPolygon
//enabled: !missionItem.followTerrain
}
// Column - Scan
/*SectionHeader {
id: polylineHeader
text: qsTr("Gateway Poly Line")
}
QGCButton {
id: polylineEditor
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: polylineInteractive ? "Done" : "Edit"
onClicked: editPolyline()
}
QGCButton {
id: swapEndpoints
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: "Swap End-Points"
onClicked: polyline.swapEndPoints()
}*/
SectionHeader {
id: statsHeader
......@@ -161,9 +85,6 @@ Rectangle {
columnSpacing: ScreenTools.defaultFontPixelWidth
visible: statsHeader.checked
/*QGCLabel { text: qsTr("Layers") }
QGCLabel { text: areaItem.layers.valueString }*/
QGCLabel { text: qsTr("Area") }
QGCLabel { text: QGroundControl.squareMetersToAppSettingsAreaUnits(areaItem.area).toFixed(2) + " " + QGroundControl.appSettingsAreaUnitsString }
......
src/comm/ros_bridge/include/server.cpp
View file @ 0510d157
......@@ -10,6 +10,7 @@ ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc)
void ros_bridge::com_private::Server::advertiseService(
const std::string &service, const std::string &type,
const Server::CallbackType &userCallback) {
std::string clientName = serviceAdvertiserKey + service;
auto it = _serviceMap.find(clientName);
if (it != _serviceMap.end())
......
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