Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
6fd60bb5
Commit
6fd60bb5
authored
Sep 24, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
CircularSurvey concurr. update improved
parent
81a0bb81
Changes
12
Hide whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
628 additions
and
487 deletions
+628
-487
search_limit.pb.h
...buntu/include/ortools/constraint_solver/search_limit.pb.h
+2
-0
qgroundcontrol.pro
qgroundcontrol.pro
+2
-0
CircularSurveyItemEditor.qml
src/PlanView/CircularSurveyItemEditor.qml
+40
-85
CSWorker.cpp
src/Wima/CSWorker.cpp
+368
-0
CSWorker.h
src/Wima/CSWorker.h
+57
-0
CircularSurvey.cc
src/Wima/CircularSurvey.cc
+80
-340
CircularSurvey.h
src/Wima/CircularSurvey.h
+13
-19
snake.cpp
src/Wima/Snake/snake.cpp
+32
-13
snake.h
src/Wima/Snake/snake.h
+6
-1
WimaPlaner.cc
src/Wima/WimaPlaner.cc
+7
-6
WimaPlaner.h
src/Wima/WimaPlaner.h
+21
-21
CircularSurveyMapVisual.qml
src/WimaView/CircularSurveyMapVisual.qml
+0
-2
No files found.
libs/or-tools-src-ubuntu/include/ortools/constraint_solver/search_limit.pb.h
View file @
6fd60bb5
// Generated by the protocol buffer compiler. DO NOT EDIT!
// Generated by the protocol buffer compiler. DO NOT EDIT!
this
->
_calculating
=
false
;
emit
calculatingChanged
();
// source: ortools/constraint_solver/search_limit.proto
// source: ortools/constraint_solver/search_limit.proto
#ifndef GOOGLE_PROTOBUF_INCLUDED_ortools_2fconstraint_5fsolver_2fsearch_5flimit_2eproto
#ifndef GOOGLE_PROTOBUF_INCLUDED_ortools_2fconstraint_5fsolver_2fsearch_5flimit_2eproto
...
...
qgroundcontrol.pro
View file @
6fd60bb5
...
@@ -413,6 +413,7 @@ FORMS += \
...
@@ -413,6 +413,7 @@ FORMS += \
#
#
HEADERS
+=
\
HEADERS
+=
\
src
/
Wima
/
CSWorker
.
h
\
src
/
Wima
/
CircularSurvey
.
h
\
src
/
Wima
/
CircularSurvey
.
h
\
src
/
Wima
/
Geometry
/
GenericCircle
.
h
\
src
/
Wima
/
Geometry
/
GenericCircle
.
h
\
src
/
Wima
/
Snake
/
clipper
/
clipper
.
hpp
\
src
/
Wima
/
Snake
/
clipper
/
clipper
.
hpp
\
...
@@ -502,6 +503,7 @@ HEADERS += \
...
@@ -502,6 +503,7 @@ HEADERS += \
src
/
comm
/
ros_bridge
/
include
/
topic_subscriber
.
h
\
src
/
comm
/
ros_bridge
/
include
/
topic_subscriber
.
h
\
src
/
comm
/
utilities
.
h
src
/
comm
/
utilities
.
h
SOURCES
+=
\
SOURCES
+=
\
src
/
Wima
/
CSWorker
.
cpp
\
src
/
Wima
/
CircularSurvey
.
cc
\
src
/
Wima
/
CircularSurvey
.
cc
\
src
/
Wima
/
Snake
/
clipper
/
clipper
.
cpp
\
src
/
Wima
/
Snake
/
clipper
/
clipper
.
cpp
\
src
/
Wima
/
Snake
/
snake
.
cpp
\
src
/
Wima
/
Snake
/
snake
.
cpp
\
...
...
src/PlanView/CircularSurveyItemEditor.qml
View file @
6fd60bb5
...
@@ -92,9 +92,6 @@ Rectangle {
...
@@ -92,9 +92,6 @@ Rectangle {
columns
:
2
columns
:
2
visible
:
transectsHeader
.
checked
visible
:
transectsHeader
.
checked
QGCLabel
{
text
:
qsTr
(
"
Delta R
"
)
}
QGCLabel
{
text
:
qsTr
(
"
Delta R
"
)
}
FactTextField
{
FactTextField
{
fact
:
missionItem
.
deltaR
fact
:
missionItem
.
deltaR
...
@@ -131,12 +128,26 @@ Rectangle {
...
@@ -131,12 +128,26 @@ Rectangle {
}
}
}
}
Column
{
GridLayout
{
anchors.left
:
parent
.
left
anchors.left
:
parent
.
left
anchors.right
:
parent
.
right
anchors.right
:
parent
.
right
spacing
:
_margin
columnSpacing
:
_margin
rowSpacing
:
_margin
columns
:
2
visible
:
transectsHeader
.
checked
visible
:
transectsHeader
.
checked
QGCButton
{
text
:
qsTr
(
"
Reverse
"
)
onClicked
:
missionItem
.
reverse
();
Layout.fillWidth
:
true
}
QGCButton
{
text
:
qsTr
(
"
Reset Ref.
"
)
onClicked
:
missionItem
.
resetReference
();
Layout.fillWidth
:
true
}
QGCCheckBox
{
QGCCheckBox
{
id
:
relAlt
id
:
relAlt
text
:
qsTr
(
"
Relative altitude
"
)
text
:
qsTr
(
"
Relative altitude
"
)
...
@@ -144,6 +155,8 @@ Rectangle {
...
@@ -144,6 +155,8 @@ Rectangle {
enabled
:
missionItem
.
cameraCalc
.
isManualCamera
&&
!
missionItem
.
followTerrain
enabled
:
missionItem
.
cameraCalc
.
isManualCamera
&&
!
missionItem
.
followTerrain
visible
:
QGroundControl
.
corePlugin
.
options
.
showMissionAbsoluteAltitude
||
(
!
missionItem
.
cameraCalc
.
distanceToSurfaceRelative
&&
!
missionItem
.
followTerrain
)
visible
:
QGroundControl
.
corePlugin
.
options
.
showMissionAbsoluteAltitude
||
(
!
missionItem
.
cameraCalc
.
distanceToSurfaceRelative
&&
!
missionItem
.
followTerrain
)
onClicked
:
missionItem
.
cameraCalc
.
distanceToSurfaceRelative
=
checked
onClicked
:
missionItem
.
cameraCalc
.
distanceToSurfaceRelative
=
checked
Layout.fillWidth
:
true
Layout.columnSpan
:
2
Connections
{
Connections
{
target
:
missionItem
.
cameraCalc
target
:
missionItem
.
cameraCalc
...
@@ -151,96 +164,38 @@ Rectangle {
...
@@ -151,96 +164,38 @@ Rectangle {
}
}
}
}
FactCheckBox
{
text
:
qsTr
(
"
Reverse Path
"
)
fact
:
missionItem
.
reverse
}
QGCButton
{
text
:
qsTr
(
"
Reset Reference
"
)
onClicked
:
missionItem
.
resetReference
();
Layout.fillWidth
:
true
}
}
}
SectionHeader
{
Column
{
id
:
miscellaneousHeader
text
:
qsTr
(
"
Miscellaneous
"
)
}
ColumnLayout
{
anchors.left
:
parent
.
left
anchors.left
:
parent
.
left
anchors.right
:
parent
.
right
anchors.right
:
parent
.
right
spacing
:
_margin
spacing
:
_margin
visible
:
miscellaneousHeader
.
checked
GridLayout
{
BusyIndicator
{
Layout.fillWidth
:
true
id
:
indicator
columnSpacing
:
_margin
anchors.horizontalCenter
:
parent
.
horizontalCenter
rowSpacing
:
_margin
running
:
missionItem
.
calculating
columns
:
2
onRunningChanged
:
{
QGCLabel
{
text
:
qsTr
(
"
Max Waypoints
"
)
}
if
(
running
){
FactTextField
{
visible
=
true
fact
:
missionItem
.
maxWaypoints
}
else
{
Layout.fillWidth
:
true
timer
.
restart
()
}
}
}
}
// GridLayout
}
// ColumnLayout
/*
Timer
{
// The following code causes seg. faults from time to time
id
:
timer
SectionHeader {
interval
:
2000
id: terrainHeader
repeat
:
false
text: qsTr("Terrain")
checked: missionItem.followTerrain
}
ColumnLayout {
onTriggered
:
{
anchors.left: parent.left
if
(
indicator
.
running
==
false
){
anchors.right: parent.right
indicator
.
visible
=
false
spacing: _margin
}
visible: terrainHeader.checked
}
QGCCheckBox {
id: followsTerrainCheckBox
text: qsTr("Vehicle follows terrain")
checked: missionItem.followTerrain
onClicked: missionItem.followTerrain = checked
}
GridLayout {
Layout.fillWidth: true
columnSpacing: _margin
rowSpacing: _margin
columns: 2
visible: followsTerrainCheckBox.checked
QGCLabel { text: qsTr("Tolerance") }
FactTextField {
fact: missionItem.terrainAdjustTolerance
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Max Climb Rate") }
FactTextField {
fact: missionItem.terrainAdjustMaxClimbRate
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Max Descent Rate") }
FactTextField {
fact: missionItem.terrainAdjustMaxDescentRate
Layout.fillWidth: true
}
}
}
}
}*/
}
/*SectionHeader {
id: statsHeader
text: qsTr("Statistics")
}*/
//TransectStyleComplexItemStats { }
}
// Column
}
// Column
}
// Rectangle
}
// Rectangle
src/Wima/CSWorker.cpp
0 → 100644
View file @
6fd60bb5
#include "CSWorker.h"
// Wima
#define CLIPPER_SCALE 10000
#include "clipper/clipper.hpp"
template
<
int
k
>
ClipperLib
::
cInt
get
(
ClipperLib
::
IntPoint
&
p
);
#include "Geometry/GenericCircle.h"
// std
#include <chrono>
// Qt
#include <QDebug>
template
<>
ClipperLib
::
cInt
get
<
0
>
(
ClipperLib
::
IntPoint
&
p
)
{
return
p
.
X
;
}
template
<>
ClipperLib
::
cInt
get
<
1
>
(
ClipperLib
::
IntPoint
&
p
)
{
return
p
.
Y
;
}
CSWorker
::
CSWorker
(
QObject
*
parent
)
:
QThread
(
parent
),
_deltaR
(
2
*
bu
::
si
::
meter
),
_deltaAlpha
(
3
*
bu
::
degree
::
degree
),
_minLength
(
10
*
bu
::
si
::
meter
),
_calculating
(
false
),
_stop
(
false
),
_restart
(
false
)
{}
CSWorker
::~
CSWorker
()
{
this
->
_stop
=
true
;
Lock
lk
(
this
->
_mutex
);
this
->
_restart
=
true
;
this
->
_cv
.
notify_one
();
lk
.
unlock
();
this
->
wait
();
}
bool
CSWorker
::
calculating
()
{
return
this
->
_calculating
;
}
void
CSWorker
::
update
(
const
QList
<
QGeoCoordinate
>
&
polygon
,
const
QGeoCoordinate
&
origin
,
snake
::
Length
deltaR
,
snake
::
Length
minLength
,
snake
::
Angle
deltaAlpha
)
{
// Sample input.
Lock
lk
(
this
->
_mutex
);
this
->
_polygon
=
polygon
;
this
->
_origin
=
origin
;
this
->
_deltaR
=
deltaR
;
this
->
_deltaAlpha
=
deltaAlpha
;
this
->
_minLength
=
minLength
;
lk
.
unlock
();
if
(
!
this
->
isRunning
())
{
this
->
start
();
}
else
{
Lock
lk
(
this
->
_mutex
);
this
->
_restart
=
true
;
this
->
_cv
.
notify_one
();
}
}
void
CSWorker
::
run
()
{
qWarning
()
<<
"CSWorker::run(): thread start."
;
while
(
!
this
->
_stop
)
{
// Copy input.
Lock
lk
(
this
->
_mutex
);
const
auto
polygon
=
this
->
_polygon
;
const
auto
origin
=
this
->
_origin
;
const
auto
deltaR
=
this
->
_deltaR
;
const
auto
deltaAlpha
=
this
->
_deltaAlpha
;
const
auto
minLength
=
this
->
_minLength
;
lk
.
unlock
();
// Check preconitions
if
(
polygon
.
size
()
>=
3
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): calculation "
"started."
;
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
const
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
using
namespace
boost
::
units
;
this
->
_calculating
=
true
;
emit
calculatingChanged
();
// Convert geo polygon to ENU polygon.
snake
::
BoostPolygon
polygonENU
;
snake
::
BoostPoint
originENU
{
0
,
0
};
snake
::
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
string
error
;
// Check validity.
if
(
!
bg
::
is_valid
(
polygonENU
,
error
))
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): "
"invalid polygon."
;
qWarning
()
<<
error
.
c_str
();
#endif
}
else
{
// Calculate polygon distances and angles.
std
::
vector
<
snake
::
Length
>
distances
;
distances
.
reserve
(
polygonENU
.
outer
().
size
());
std
::
vector
<
snake
::
Angle
>
angles
;
angles
.
reserve
(
polygonENU
.
outer
().
size
());
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run():"
;
#endif
for
(
const
auto
&
p
:
polygonENU
.
outer
())
{
snake
::
Length
distance
=
bg
::
distance
(
originENU
,
p
)
*
si
::
meter
;
distances
.
push_back
(
distance
);
snake
::
Angle
alpha
=
(
std
::
atan2
(
p
.
get
<
1
>
(),
p
.
get
<
0
>
()))
*
si
::
radian
;
alpha
=
alpha
<
0
*
si
::
radian
?
alpha
+
2
*
M_PI
*
si
::
radian
:
alpha
;
angles
.
push_back
(
alpha
);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"distances, angles, coordinates:"
;
qWarning
()
<<
to_string
(
distance
).
c_str
();
qWarning
()
<<
to_string
(
snake
::
Degree
(
alpha
)).
c_str
();
qWarning
()
<<
"x = "
<<
p
.
get
<
0
>
()
<<
"y = "
<<
p
.
get
<
1
>
();
#endif
}
auto
rMin
=
deltaR
;
// minimal circle radius
snake
::
Angle
alpha1
(
0
*
degree
::
degree
);
snake
::
Angle
alpha2
(
360
*
degree
::
degree
);
// Determine r_min by successive approximation
if
(
!
bg
::
within
(
originENU
,
polygonENU
))
{
rMin
=
bg
::
distance
(
originENU
,
polygonENU
)
*
si
::
meter
;
}
auto
rMax
=
(
*
std
::
max_element
(
distances
.
begin
(),
distances
.
end
()));
// maximal circle radius
// Scale parameters and coordinates.
const
auto
rMinScaled
=
ClipperLib
::
cInt
(
std
::
round
(
rMin
.
value
()
*
CLIPPER_SCALE
));
const
auto
deltaRScaled
=
ClipperLib
::
cInt
(
std
::
round
(
deltaR
.
value
()
*
CLIPPER_SCALE
));
auto
originScaled
=
ClipperLib
::
IntPoint
{
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
0
>
())),
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
1
>
()))};
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto
s1
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
// Generate circle sectors.
auto
rScaled
=
rMinScaled
;
const
auto
nTran
=
long
(
std
::
ceil
(((
rMax
-
rMin
)
/
deltaR
).
value
()));
vector
<
ClipperLib
::
Path
>
sectors
(
nTran
,
ClipperLib
::
Path
());
const
auto
nSectors
=
long
(
std
::
round
(((
alpha2
-
alpha1
)
/
deltaAlpha
).
value
()));
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): sector parameres:"
;
qWarning
()
<<
"alpha1: "
<<
to_string
(
snake
::
Degree
(
alpha1
)).
c_str
();
qWarning
()
<<
"alpha2: "
<<
to_string
(
snake
::
Degree
(
alpha2
)).
c_str
();
qWarning
()
<<
"n: "
<<
to_string
((
alpha2
-
alpha1
)
/
deltaAlpha
).
c_str
();
qWarning
()
<<
"nSectors: "
<<
nSectors
;
qWarning
()
<<
"rMin: "
<<
to_string
(
rMin
).
c_str
();
qWarning
()
<<
"rMax: "
<<
to_string
(
rMax
).
c_str
();
qWarning
()
<<
"nTran: "
<<
nTran
;
#endif
using
ClipperCircle
=
GenericCircle
<
ClipperLib
::
cInt
,
ClipperLib
::
IntPoint
>
;
for
(
auto
&
sector
:
sectors
)
{
ClipperCircle
circle
(
rScaled
,
originScaled
);
approximate
(
circle
,
nSectors
,
sector
);
rScaled
+=
deltaRScaled
;
}
// Clip sectors to polygonENU.
ClipperLib
::
Path
polygonClipper
;
auto
&
outer
=
polygonENU
.
outer
();
polygonClipper
.
reserve
(
outer
.
size
()
-
1
);
for
(
auto
it
=
outer
.
begin
();
it
<
outer
.
end
()
-
1
;
++
it
)
{
auto
x
=
ClipperLib
::
cInt
(
std
::
round
(
it
->
get
<
0
>
()
*
CLIPPER_SCALE
));
auto
y
=
ClipperLib
::
cInt
(
std
::
round
(
it
->
get
<
1
>
()
*
CLIPPER_SCALE
));
polygonClipper
.
push_back
(
ClipperLib
::
IntPoint
{
x
,
y
});
}
ClipperLib
::
Clipper
clipper
;
clipper
.
AddPath
(
polygonClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
AddPaths
(
sectors
,
ClipperLib
::
ptSubject
,
false
);
ClipperLib
::
PolyTree
transectsClipper
;
clipper
.
Execute
(
ClipperLib
::
ctIntersection
,
transectsClipper
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
// Extract transects from PolyTree and convert them to
// BoostLineString
snake
::
Transects
transectsENU
;
for
(
const
auto
&
child
:
transectsClipper
.
Childs
)
{
snake
::
BoostLineString
transect
;
transect
.
reserve
(
child
->
Contour
.
size
());
for
(
const
auto
&
vertex
:
child
->
Contour
)
{
auto
x
=
static_cast
<
double
>
(
vertex
.
X
)
/
CLIPPER_SCALE
;
auto
y
=
static_cast
<
double
>
(
vertex
.
Y
)
/
CLIPPER_SCALE
;
transect
.
push_back
(
snake
::
BoostPoint
(
x
,
y
));
}
transectsENU
.
push_back
(
transect
);
}
// Join sectors which where slit due to clipping.
const
double
th
=
0.01
;
for
(
auto
ito
=
transectsENU
.
begin
();
ito
<
transectsENU
.
end
();
++
ito
)
{
for
(
auto
iti
=
ito
+
1
;
iti
<
transectsENU
.
end
();
++
iti
)
{
auto
dist1
=
bg
::
distance
(
ito
->
front
(),
iti
->
front
());
if
(
dist1
<
th
)
{
snake
::
BoostLineString
temp
;
for
(
auto
it
=
iti
->
end
()
-
1
;
it
>=
iti
->
begin
();
--
it
)
{
temp
.
push_back
(
*
it
);
}
temp
.
insert
(
temp
.
end
(),
ito
->
begin
(),
ito
->
end
());
*
ito
=
temp
;
transectsENU
.
erase
(
iti
);
break
;
}
auto
dist2
=
bg
::
distance
(
ito
->
front
(),
iti
->
back
());
if
(
dist2
<
th
)
{
snake
::
BoostLineString
temp
;
temp
.
insert
(
temp
.
end
(),
iti
->
begin
(),
iti
->
end
());
temp
.
insert
(
temp
.
end
(),
ito
->
begin
(),
ito
->
end
());
*
ito
=
temp
;
transectsENU
.
erase
(
iti
);
break
;
}
auto
dist3
=
bg
::
distance
(
ito
->
back
(),
iti
->
front
());
if
(
dist3
<
th
)
{
snake
::
BoostLineString
temp
;
temp
.
insert
(
temp
.
end
(),
ito
->
begin
(),
ito
->
end
());
temp
.
insert
(
temp
.
end
(),
iti
->
begin
(),
iti
->
end
());
*
ito
=
temp
;
transectsENU
.
erase
(
iti
);
break
;
}
auto
dist4
=
bg
::
distance
(
ito
->
back
(),
iti
->
back
());
if
(
dist4
<
th
)
{
snake
::
BoostLineString
temp
;
temp
.
insert
(
temp
.
end
(),
ito
->
begin
(),
ito
->
end
());
for
(
auto
it
=
iti
->
end
()
-
1
;
it
>=
iti
->
begin
();
--
it
)
{
temp
.
push_back
(
*
it
);
}
*
ito
=
temp
;
transectsENU
.
erase
(
iti
);
break
;
}
}
}
// Remove short transects
for
(
auto
it
=
transectsENU
.
begin
();
it
<
transectsENU
.
end
();)
{
if
(
bg
::
length
(
*
it
)
<
minLength
.
value
())
{
it
=
transectsENU
.
erase
(
it
);
}
else
{
++
it
;
}
}
// Move transect with min. distance to the front.
auto
minDist
=
std
::
numeric_limits
<
double
>::
max
();
auto
minIt
=
transectsENU
.
begin
();
bool
reverse
=
false
;
for
(
auto
it
=
transectsENU
.
begin
();
it
<
transectsENU
.
end
();
++
it
)
{
auto
distFront
=
bg
::
distance
(
originENU
,
it
->
front
());
auto
distBack
=
bg
::
distance
(
originENU
,
it
->
back
());
if
(
distFront
<
minDist
)
{
minDist
=
distFront
;
minIt
=
it
;
reverse
=
false
;
}
if
(
distBack
<
minDist
)
{
minDist
=
distBack
;
minIt
=
it
;
reverse
=
true
;
}
}
// Swap and reverse (if necessary).
if
(
minIt
!=
transectsENU
.
begin
())
{
auto
minTransect
=
*
minIt
;
if
(
reverse
)
{
snake
::
BoostLineString
rev
;
for
(
auto
it
=
minTransect
.
end
()
-
1
;
it
>=
minTransect
.
begin
();
--
it
)
{
rev
.
push_back
(
*
it
);
}
minTransect
=
rev
;
}
*
minIt
=
*
transectsENU
.
begin
();
*
transectsENU
.
begin
()
=
minTransect
;
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning
()
<<
"CSWorker::run(): transect gen. time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
s1
)
.
count
()
<<
" ms"
;
#endif
if
(
transectsENU
.
size
()
==
0
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): "
"not able to generate transects."
;
#endif
}
else
if
(
this
->
_restart
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): "
"restart requested."
;
#endif
}
else
{
// Prepare data for routing.
std
::
vector
<
snake
::
TransectInfo
>
transectsInfo
;
snake
::
Route
route
;
const
auto
routingStart
=
std
::
chrono
::
high_resolution_clock
::
now
();
const
auto
maxRoutingTime
=
std
::
chrono
::
minutes
(
1
);
const
auto
routingEnd
=
routingStart
+
maxRoutingTime
;
const
auto
&
restart
=
this
->
_restart
;
auto
stopLambda
=
[
&
restart
,
routingEnd
]
{
bool
expired
=
std
::
chrono
::
high_resolution_clock
::
now
()
>
routingEnd
;
return
restart
||
expired
;
};
std
::
string
errorString
;
// Route transects;
bool
success
=
snake
::
route
(
polygonENU
,
transectsENU
,
transectsInfo
,
route
,
stopLambda
,
errorString
);
if
(
!
success
&&
!
this
->
_restart
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): "
"routing failed."
;
#endif
}
else
if
(
this
->
_restart
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): "
"restart requested."
;
#endif
}
else
{
// Remove return path.
const
auto
&
info
=
transectsInfo
.
back
();
const
auto
&
lastTransect
=
transectsENU
[
info
.
index
];
const
auto
&
lastWaypoint
=
info
.
reversed
?
lastTransect
.
front
()
:
lastTransect
.
back
();
auto
&
wp
=
route
.
back
();
while
(
wp
!=
lastWaypoint
)
{
route
.
pop_back
();
wp
=
route
.
back
();
}
// Convert to geo coordinates and notify main thread.
auto
pRoute
=
PtrRoute
(
new
Route
());
for
(
const
auto
&
vertex
:
route
)
{
QGeoCoordinate
c
;
snake
::
fromENU
(
origin
,
vertex
,
c
);
pRoute
->
append
(
c
);
}
emit
ready
(
pRoute
);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CSWorker::run(): "
"concurrent update success."
;
#endif
}
}
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning
()
<<
"CSWorker::run(): execution time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
.
count
()
<<
" ms"
;
#endif
this
->
_calculating
=
false
;
emit
calculatingChanged
();
}
#ifdef DEBUG_CIRCULAR_SURVEY
else
{
qWarning
()
<<
"CSWorker::run(): preconditions failed."
;
}
#endif
Lock
lk2
(
this
->
_mutex
);
if
(
!
this
->
_restart
)
{
this
->
_cv
.
wait
(
lk2
,
[
this
]
{
return
this
->
_restart
.
load
();
});
}
this
->
_restart
=
false
;
}
qWarning
()
<<
"CSWorker::run(): thread end."
;
}
src/Wima/CSWorker.h
0 → 100644
View file @
6fd60bb5
#pragma once
#include <QGeoCoordinate>
#include <QList>
#include <QSharedPointer>
#include <QThread>
#include "snake.h"
#include <atomic>
#include <condition_variable>
#include <mutex>
//!
//! \brief The CSWorker class
//! \note Don't call QThread::start, QThread::quit, etc. onyl use Worker
//! members!
class
CSWorker
:
public
QThread
{
Q_OBJECT
using
Lock
=
std
::
unique_lock
<
std
::
mutex
>
;
public:
using
Route
=
QList
<
QGeoCoordinate
>
;
using
PtrRoute
=
QSharedPointer
<
Route
>
;
CSWorker
(
QObject
*
parent
=
nullptr
);
~
CSWorker
()
override
;
bool
calculating
();
public
slots
:
void
update
(
const
QList
<
QGeoCoordinate
>
&
polygon
,
const
QGeoCoordinate
&
origin
,
snake
::
Length
deltaR
,
snake
::
Length
minLength
,
snake
::
Angle
deltaAlpha
);
signals:
void
ready
(
PtrRoute
pTransects
);
void
calculatingChanged
();
protected:
void
run
()
override
;
private:
mutable
std
::
mutex
_mutex
;
mutable
std
::
condition_variable
_cv
;
// Internal data
QList
<
QGeoCoordinate
>
_polygon
;
QGeoCoordinate
_origin
;
snake
::
Length
_deltaR
;
snake
::
Angle
_deltaAlpha
;
snake
::
Length
_minLength
;
std
::
size_t
_maxWaypoints
;
// State
std
::
atomic_bool
_calculating
;
std
::
atomic_bool
_stop
;
std
::
atomic_bool
_restart
;
};
src/Wima/CircularSurvey.cc
View file @
6fd60bb5
#
include
"CircularSurvey.h"
#
include
"CircularSurvey.h"
#include "CSWorker.h"
#include <QtConcurrentRun>
// QGC
#include "JsonHelper.h"
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "QGCApplication.h"
// Wima
#include <chrono>
#include "clipper/clipper.hpp"
#include "snake.h"
#include "snake.h"
#define CLIPPER_SCALE 10000
// boost
template
<
int
k
>
ClipperLib
::
cInt
get
(
ClipperLib
::
IntPoint
&
p
);
#include "Geometry/GenericCircle.h"
#include "Geometry/GeoUtilities.h"
#include "Geometry/PlanimetryCalculus.h"
#include "Geometry/PolygonCalculus.h"
#include <boost/units/io.hpp>
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si.hpp>
template
<>
ClipperLib
::
cInt
get
<
0
>
(
ClipperLib
::
IntPoint
&
p
)
{
return
p
.
X
;
}
template
<>
ClipperLib
::
cInt
get
<
1
>
(
ClipperLib
::
IntPoint
&
p
)
{
return
p
.
Y
;
}
template
<
class
Functor
>
class
CommandRAII
{
template
<
class
Functor
>
class
CommandRAII
{
public:
public:
CommandRAII
(
Functor
f
)
:
fun
(
f
)
{}
CommandRAII
(
Functor
f
)
:
fun
(
f
)
{}
...
@@ -36,8 +22,6 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
...
@@ -36,8 +22,6 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const
char
*
CircularSurvey
::
deltaRName
=
"DeltaR"
;
const
char
*
CircularSurvey
::
deltaRName
=
"DeltaR"
;
const
char
*
CircularSurvey
::
deltaAlphaName
=
"DeltaAlpha"
;
const
char
*
CircularSurvey
::
deltaAlphaName
=
"DeltaAlpha"
;
const
char
*
CircularSurvey
::
transectMinLengthName
=
"TransectMinLength"
;
const
char
*
CircularSurvey
::
transectMinLengthName
=
"TransectMinLength"
;
const
char
*
CircularSurvey
::
reverseName
=
"Reverse"
;
const
char
*
CircularSurvey
::
maxWaypointsName
=
"MaxWaypoints"
;
const
char
*
CircularSurvey
::
CircularSurveyName
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
CircularSurveyName
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
refPointLatitudeName
=
"ReferencePointLat"
;
const
char
*
CircularSurvey
::
refPointLatitudeName
=
"ReferencePointLat"
;
const
char
*
CircularSurvey
::
refPointLongitudeName
=
"ReferencePointLong"
;
const
char
*
CircularSurvey
::
refPointLongitudeName
=
"ReferencePointLong"
;
...
@@ -52,51 +36,40 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
...
@@ -52,51 +36,40 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
_deltaR
(
settingsGroup
,
_metaDataMap
[
deltaRName
]),
_deltaR
(
settingsGroup
,
_metaDataMap
[
deltaRName
]),
_deltaAlpha
(
settingsGroup
,
_metaDataMap
[
deltaAlphaName
]),
_deltaAlpha
(
settingsGroup
,
_metaDataMap
[
deltaAlphaName
]),
_minLength
(
settingsGroup
,
_metaDataMap
[
transectMinLengthName
]),
_minLength
(
settingsGroup
,
_metaDataMap
[
transectMinLengthName
]),
_reverse
(
settingsGroup
,
_metaDataMap
[
reverseName
]),
_isInitialized
(
false
),
_pWorker
(
std
::
make_unique
<
CSWorker
>
()),
_maxWaypoints
(
settingsGroup
,
_metaDataMap
[
maxWaypointsName
]),
_needsStoring
(
false
),
_needsReversal
(
false
)
{
_isInitialized
(
false
),
_calculating
(
false
),
_cancle
(
false
)
{
Q_UNUSED
(
kmlOrShpFile
)
Q_UNUSED
(
kmlOrShpFile
)
_editorQml
=
"qrc:/qml/CircularSurveyItemEditor.qml"
;
_editorQml
=
"qrc:/qml/CircularSurveyItemEditor.qml"
;
// Defer update if facts or ref. changes.
// Connect facts.
connect
(
&
_deltaR
,
&
Fact
::
valueChanged
,
this
,
&
CircularSurvey
::
_deferUpdate
);
connect
(
&
_deltaR
,
&
Fact
::
valueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
&
_deltaAlpha
,
&
Fact
::
valueChanged
,
this
,
connect
(
&
_deltaAlpha
,
&
Fact
::
valueChanged
,
this
,
&
CircularSurvey
::
_
deferUpdate
);
&
CircularSurvey
::
_
rebuildTransects
);
connect
(
&
_minLength
,
&
Fact
::
valueChanged
,
this
,
connect
(
&
_minLength
,
&
Fact
::
valueChanged
,
this
,
&
CircularSurvey
::
_deferUpdate
);
&
CircularSurvey
::
_rebuildTransects
);
connect
(
&
_maxWaypoints
,
&
Fact
::
valueChanged
,
[
this
]
{
this
->
CircularSurvey
::
_deferUpdate
();
qWarning
()
<<
"max waypoints implementaion missing"
;
});
connect
(
&
_reverse
,
&
Fact
::
valueChanged
,
[
this
]
{
this
->
CircularSurvey
::
_deferUpdate
();
qWarning
()
<<
"reverse implementaion missing"
;
});
connect
(
this
,
&
CircularSurvey
::
refPointChanged
,
this
,
connect
(
this
,
&
CircularSurvey
::
refPointChanged
,
this
,
&
CircularSurvey
::
_deferUpdate
);
&
CircularSurvey
::
_rebuildTransects
);
connect
(
&
this
->
_surveyAreaPolygon
,
&
QGCMapPolygon
::
pathChanged
,
this
,
// Connect worker.
&
CircularSurvey
::
_deferUpdate
);
qRegisterMetaType
<
PtrRoute
>
(
"PtrRoute"
);
// Setup Timer.
connect
(
this
->
_pWorker
.
get
(),
&
CSWorker
::
ready
,
this
,
_timer
.
setSingleShot
(
true
);
&
CircularSurvey
::
_setTransects
);
connect
(
&
_timer
,
&
QTimer
::
timeout
,
[
this
]
{
this
->
_rebuildTransects
();
});
connect
(
this
->
_pWorker
.
get
(),
&
CSWorker
::
calculatingChanged
,
this
,
&
CircularSurvey
::
calculatingChanged
);
// Future watcher.
this
->
_transectsDirty
=
false
;
connect
(
&
_watcher
,
&
Watcher
::
finished
,
[
this
]
{
this
->
_calculating
=
false
;
emit
calculatingChanged
();
if
(
!
_cancle
)
{
this
->
_transectsDirty
=
false
;
}
else
{
_cancle
=
false
;
}
this
->
_rebuildTransects
();
});
}
}
CircularSurvey
::~
CircularSurvey
()
{}
void
CircularSurvey
::
resetReference
()
{
void
CircularSurvey
::
resetReference
()
{
setRefPoint
(
_surveyAreaPolygon
.
center
());
setRefPoint
(
_surveyAreaPolygon
.
center
());
}
}
void
CircularSurvey
::
reverse
()
{
this
->
_needsReversal
=
true
;
this
->
_rebuildTransects
();
}
void
CircularSurvey
::
setRefPoint
(
const
QGeoCoordinate
&
refPt
)
{
void
CircularSurvey
::
setRefPoint
(
const
QGeoCoordinate
&
refPt
)
{
if
(
refPt
!=
_referencePoint
)
{
if
(
refPt
!=
_referencePoint
)
{
_referencePoint
=
refPt
;
_referencePoint
=
refPt
;
...
@@ -123,8 +96,8 @@ bool CircularSurvey::isInitialized() { return _isInitialized; }
...
@@ -123,8 +96,8 @@ bool CircularSurvey::isInitialized() { return _isInitialized; }
bool
CircularSurvey
::
load
(
const
QJsonObject
&
complexObject
,
int
sequenceNumber
,
bool
CircularSurvey
::
load
(
const
QJsonObject
&
complexObject
,
int
sequenceNumber
,
QString
&
errorString
)
{
QString
&
errorString
)
{
// We need to pull version first to determine what validation/conversion
needs
// We need to pull version first to determine what validation/conversion
// to be performed
//
needs
to be performed
QList
<
JsonHelper
::
KeyValidateInfo
>
versionKeyInfoList
=
{
QList
<
JsonHelper
::
KeyValidateInfo
>
versionKeyInfoList
=
{
{
JsonHelper
::
jsonVersionKey
,
QJsonValue
::
Double
,
true
},
{
JsonHelper
::
jsonVersionKey
,
QJsonValue
::
Double
,
true
},
};
};
...
@@ -145,7 +118,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
...
@@ -145,7 +118,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
{
deltaRName
,
QJsonValue
::
Double
,
true
},
{
deltaRName
,
QJsonValue
::
Double
,
true
},
{
deltaAlphaName
,
QJsonValue
::
Double
,
true
},
{
deltaAlphaName
,
QJsonValue
::
Double
,
true
},
{
transectMinLengthName
,
QJsonValue
::
Double
,
true
},
{
transectMinLengthName
,
QJsonValue
::
Double
,
true
},
{
reverseName
,
QJsonValue
::
Bool
,
true
},
{
refPointLatitudeName
,
QJsonValue
::
Double
,
true
},
{
refPointLatitudeName
,
QJsonValue
::
Double
,
true
},
{
refPointLongitudeName
,
QJsonValue
::
Double
,
true
},
{
refPointLongitudeName
,
QJsonValue
::
Double
,
true
},
{
refPointAltitudeName
,
QJsonValue
::
Double
,
true
},
{
refPointAltitudeName
,
QJsonValue
::
Double
,
true
},
...
@@ -160,11 +132,11 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
...
@@ -160,11 +132,11 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
complexObject
[
ComplexMissionItem
::
jsonComplexItemTypeKey
].
toString
();
complexObject
[
ComplexMissionItem
::
jsonComplexItemTypeKey
].
toString
();
if
(
itemType
!=
VisualMissionItem
::
jsonTypeComplexItemValue
||
if
(
itemType
!=
VisualMissionItem
::
jsonTypeComplexItemValue
||
complexType
!=
CircularSurveyName
)
{
complexType
!=
CircularSurveyName
)
{
errorString
=
errorString
=
tr
(
"%1 does not support loading this complex mission item "
tr
(
"%1 does not support loading this complex mission item
type: %2:%3"
)
"
type: %2:%3"
)
.
arg
(
qgcApp
()
->
applicationName
())
.
arg
(
qgcApp
()
->
applicationName
())
.
arg
(
itemType
)
.
arg
(
itemType
)
.
arg
(
complexType
);
.
arg
(
complexType
);
return
false
;
return
false
;
}
}
...
@@ -189,7 +161,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
...
@@ -189,7 +161,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_referencePoint
.
setLongitude
(
complexObject
[
refPointLongitudeName
].
toDouble
());
_referencePoint
.
setLongitude
(
complexObject
[
refPointLongitudeName
].
toDouble
());
_referencePoint
.
setLatitude
(
complexObject
[
refPointLatitudeName
].
toDouble
());
_referencePoint
.
setLatitude
(
complexObject
[
refPointLatitudeName
].
toDouble
());
_referencePoint
.
setAltitude
(
complexObject
[
refPointAltitudeName
].
toDouble
());
_referencePoint
.
setAltitude
(
complexObject
[
refPointAltitudeName
].
toDouble
());
_reverse
.
setRawValue
(
complexObject
[
reverseName
].
toBool
());
setIsInitialized
(
true
);
setIsInitialized
(
true
);
_ignoreRecalc
=
false
;
_ignoreRecalc
=
false
;
...
@@ -220,7 +191,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
...
@@ -220,7 +191,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
saveObject
[
deltaRName
]
=
_deltaR
.
rawValue
().
toDouble
();
saveObject
[
deltaRName
]
=
_deltaR
.
rawValue
().
toDouble
();
saveObject
[
deltaAlphaName
]
=
_deltaAlpha
.
rawValue
().
toDouble
();
saveObject
[
deltaAlphaName
]
=
_deltaAlpha
.
rawValue
().
toDouble
();
saveObject
[
transectMinLengthName
]
=
_minLength
.
rawValue
().
toDouble
();
saveObject
[
transectMinLengthName
]
=
_minLength
.
rawValue
().
toDouble
();
saveObject
[
reverseName
]
=
_reverse
.
rawValue
().
toBool
();
saveObject
[
refPointLongitudeName
]
=
_referencePoint
.
longitude
();
saveObject
[
refPointLongitudeName
]
=
_referencePoint
.
longitude
();
saveObject
[
refPointLatitudeName
]
=
_referencePoint
.
latitude
();
saveObject
[
refPointLatitudeName
]
=
_referencePoint
.
latitude
();
saveObject
[
refPointAltitudeName
]
=
_referencePoint
.
altitude
();
saveObject
[
refPointAltitudeName
]
=
_referencePoint
.
altitude
();
...
@@ -279,8 +249,8 @@ void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
...
@@ -279,8 +249,8 @@ void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
for
(
const
CoordInfo_t
&
transectCoordInfo
:
transect
)
{
for
(
const
CoordInfo_t
&
transectCoordInfo
:
transect
)
{
item
=
new
MissionItem
(
item
=
new
MissionItem
(
seqNum
++
,
MAV_CMD_NAV_WAYPOINT
,
mavFrame
,
seqNum
++
,
MAV_CMD_NAV_WAYPOINT
,
mavFrame
,
0
,
// Hold time (delay for hover and capture to settle vehicle befor
e
0
,
// Hold time (delay for hover and capture to settle vehicl
e
//
image is taken)
// before
image is taken)
0.0
,
// No acceptance radius specified
0.0
,
// No acceptance radius specified
0.0
,
// Pass through waypoint
0.0
,
// Pass through waypoint
std
::
numeric_limits
<
double
>::
quiet_NaN
(),
// Yaw unchanged
std
::
numeric_limits
<
double
>::
quiet_NaN
(),
// Yaw unchanged
...
@@ -318,260 +288,43 @@ bool CircularSurvey::readyForSave() const {
...
@@ -318,260 +288,43 @@ bool CircularSurvey::readyForSave() const {
double
CircularSurvey
::
additionalTimeDelay
()
const
{
return
0
;
}
double
CircularSurvey
::
additionalTimeDelay
()
const
{
return
0
;
}
void
CircularSurvey
::
_rebuildTransectsPhase1
(
void
)
{
void
CircularSurvey
::
_rebuildTransectsPhase1
(
void
)
{
if
(
_calculating
)
if
(
this
->
_needsStoring
)
{
return
;
// If the transects are getting rebuilt then any previously loaded
if
(
!
_transectsDirty
)
{
// mission items are now invalid.
auto
pTransects
=
_watcher
.
result
();
if
(
_loadedMissionItemsParent
)
{
if
(
pTransects
)
{
_loadedMissionItems
.
clear
();
#ifdef DEBUG_CIRCULAR_SURVEY
_loadedMissionItemsParent
->
deleteLater
();
qWarning
()
_loadedMissionItemsParent
=
nullptr
;
<<
"CircularSurvey::_rebuildTransectsPhase1(): storing transects."
;
#endif
// If the transects are getting rebuilt then any previously loaded
// mission items are now invalid.
if
(
_loadedMissionItemsParent
)
{
_loadedMissionItems
.
clear
();
_loadedMissionItemsParent
->
deleteLater
();
_loadedMissionItemsParent
=
nullptr
;
}
// Store new transects;
_transects
=
*
pTransects
;
}
}
}
else
{
// Copy Transects.
_transects
.
clear
();
QList
<
CoordInfo_t
>
list
;
// Check preconitions
for
(
const
auto
c
:
*
this
->
_pRoute
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
list
.
append
(
CoordInfo_t
{
c
,
CoordTypeInterior
});
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): checking preconditions."
;
#endif
if
(
_isInitialized
&&
_surveyAreaPolygon
.
count
()
>=
3
&&
_deltaAlpha
.
rawValue
()
<=
_deltaAlpha
.
rawMax
()
&&
_deltaAlpha
.
rawValue
()
>=
_deltaAlpha
.
rawMin
()
&&
_deltaR
.
rawValue
()
<=
_deltaR
.
rawMax
()
&&
_deltaR
.
rawValue
()
>=
_deltaR
.
rawMin
())
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): preconditions ok."
;
#endif
using
namespace
boost
::
units
;
_calculating
=
true
;
emit
calculatingChanged
();
// Copy input.
const
auto
&
polygon
=
this
->
_surveyAreaPolygon
.
coordinateList
();
const
auto
&
origin
=
this
->
_referencePoint
;
const
snake
::
Length
deltaR
(
this
->
_deltaR
.
rawValue
().
toDouble
()
*
si
::
meter
);
const
snake
::
Angle
deltaAlpha
(
this
->
_deltaAlpha
.
rawValue
().
toDouble
()
*
degree
::
degree
);
const
snake
::
Length
minLength
(
this
->
_minLength
.
rawValue
().
toDouble
()
*
si
::
meter
);
const
auto
maxWaypoints
(
this
->
_maxWaypoints
.
rawValue
().
toUInt
());
auto
future
=
QtConcurrent
::
run
([
polygon
,
origin
,
deltaAlpha
,
minLength
,
maxWaypoints
,
deltaR
]
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): calculation "
"started."
;
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
onExit
=
[
&
start
]
{
qWarning
()
<<
"CircularSurvey: concurrent update execution time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
.
count
()
<<
" ms"
;
};
CommandRAII
<
decltype
(
onExit
)
>
commandRAII
(
onExit
);
#endif
// Convert geo polygon to ENU polygon.
snake
::
BoostPolygon
polygonENU
;
snake
::
BoostPoint
originENU
{
0
,
0
};
snake
::
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
string
error
;
// Check validity.
if
(
!
bg
::
is_valid
(
polygonENU
,
error
))
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
"invalid polygon."
;
qWarning
()
<<
error
.
c_str
();
#endif
return
PtrTransects
();
}
else
{
// Calculate polygon distances and angles.
std
::
vector
<
snake
::
Length
>
distances
;
distances
.
reserve
(
polygonENU
.
outer
().
size
());
std
::
vector
<
snake
::
Angle
>
angles
;
angles
.
reserve
(
polygonENU
.
outer
().
size
());
for
(
const
auto
&
p
:
polygonENU
.
outer
())
{
snake
::
Length
distance
=
bg
::
distance
(
originENU
,
p
)
*
si
::
meter
;
distances
.
push_back
(
distance
);
snake
::
Angle
alpha
=
(
std
::
atan2
(
p
.
get
<
1
>
(),
p
.
get
<
0
>
()))
*
si
::
radian
;
alpha
=
alpha
<
0
*
si
::
radian
?
alpha
+
2
*
M_PI
*
si
::
radian
:
alpha
;
angles
.
push_back
(
alpha
);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"distances, angles, coordinates:"
;
qWarning
()
<<
to_string
(
distance
).
c_str
();
qWarning
()
<<
to_string
(
snake
::
Degree
(
alpha
)).
c_str
();
qWarning
()
<<
"x = "
<<
p
.
get
<
0
>
()
<<
"y = "
<<
p
.
get
<
1
>
();
#endif
}
auto
rMin
=
deltaR
;
// minimal circle radius
snake
::
Angle
alpha1
(
0
*
degree
::
degree
);
snake
::
Angle
alpha2
(
360
*
degree
::
degree
);
// Determine r_min by successive approximation
if
(
!
bg
::
within
(
originENU
,
polygonENU
))
{
rMin
=
bg
::
distance
(
originENU
,
polygonENU
)
*
si
::
meter
;
}
auto
rMax
=
(
*
std
::
max_element
(
distances
.
begin
(),
distances
.
end
()));
// maximal circle radius
// Scale parameters and coordinates.
const
auto
rMinScaled
=
ClipperLib
::
cInt
(
std
::
round
(
rMin
.
value
()
*
CLIPPER_SCALE
));
const
auto
deltaRScaled
=
ClipperLib
::
cInt
(
std
::
round
(
deltaR
.
value
()
*
CLIPPER_SCALE
));
auto
originScaled
=
ClipperLib
::
IntPoint
{
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
0
>
())),
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
1
>
()))};
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto
s1
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
// Generate circle sectors.
auto
rScaled
=
rMinScaled
;
const
auto
nTran
=
long
(
std
::
ceil
(((
rMax
-
rMin
)
/
deltaR
).
value
()));
vector
<
ClipperLib
::
Path
>
sectors
(
nTran
,
ClipperLib
::
Path
());
const
auto
nSectors
=
long
(
std
::
round
(((
alpha2
-
alpha1
)
/
deltaAlpha
).
value
()));
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"sector parameres:"
;
qWarning
()
<<
"alpha1: "
<<
to_string
(
snake
::
Degree
(
alpha1
)).
c_str
();
qWarning
()
<<
"alpha2: "
<<
to_string
(
snake
::
Degree
(
alpha2
)).
c_str
();
qWarning
()
<<
"n: "
<<
to_string
((
alpha2
-
alpha1
)
/
deltaAlpha
).
c_str
();
qWarning
()
<<
"nSectors: "
<<
nSectors
;
qWarning
()
<<
"rMin: "
<<
to_string
(
rMin
).
c_str
();
qWarning
()
<<
"rMax: "
<<
to_string
(
rMax
).
c_str
();
qWarning
()
<<
"nTran: "
<<
nTran
;
#endif
using
ClipperCircle
=
GenericCircle
<
ClipperLib
::
cInt
,
ClipperLib
::
IntPoint
>
;
for
(
auto
&
sector
:
sectors
)
{
ClipperCircle
circle
(
rScaled
,
originScaled
);
approximate
(
circle
,
nSectors
,
sector
);
rScaled
+=
deltaRScaled
;
}
// Clip sectors to polygonENU.
ClipperLib
::
Path
polygonClipper
;
auto
&
outer
=
polygonENU
.
outer
();
polygonClipper
.
reserve
(
outer
.
size
()
-
1
);
for
(
auto
it
=
outer
.
begin
();
it
<
outer
.
end
()
-
1
;
++
it
)
{
auto
x
=
ClipperLib
::
cInt
(
std
::
round
(
it
->
get
<
0
>
()
*
CLIPPER_SCALE
));
auto
y
=
ClipperLib
::
cInt
(
std
::
round
(
it
->
get
<
1
>
()
*
CLIPPER_SCALE
));
polygonClipper
.
push_back
(
ClipperLib
::
IntPoint
{
x
,
y
});
}
ClipperLib
::
Clipper
clipper
;
clipper
.
AddPath
(
polygonClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
AddPaths
(
sectors
,
ClipperLib
::
ptSubject
,
false
);
ClipperLib
::
PolyTree
transectsClipper
;
clipper
.
Execute
(
ClipperLib
::
ctIntersection
,
transectsClipper
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
// Extract transects from PolyTree and convert them to
// BoostLineString
snake
::
Transects
transectsENU
;
for
(
const
auto
&
child
:
transectsClipper
.
Childs
)
{
snake
::
BoostLineString
transect
;
transect
.
reserve
(
child
->
Contour
.
size
());
for
(
const
auto
&
vertex
:
child
->
Contour
)
{
auto
x
=
static_cast
<
double
>
(
vertex
.
X
)
/
CLIPPER_SCALE
;
auto
y
=
static_cast
<
double
>
(
vertex
.
Y
)
/
CLIPPER_SCALE
;
transect
.
push_back
(
snake
::
BoostPoint
(
x
,
y
));
}
if
(
bg
::
length
(
transect
)
>=
minLength
.
value
())
{
transectsENU
.
push_back
(
transect
);
}
}
// Join sectors which where slit due to clipping.
static_assert
(
false
,
"continue here."
);
for
(
std
::
size_t
i
=
0
;
i
<
transectsENU
.
size
()
-
1
;
++
i
)
{
const
auto
&
t1
=
transectsENU
[
i
];
for
(
std
::
size_t
j
=
i
+
1
;
j
<
transectsENU
.
size
();
++
j
)
{
const
auto
&
t2
=
transectsENU
[
j
];
}
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning
()
<<
"CircularSurvey: concurrent update transect gen. time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
s1
)
.
count
()
<<
" ms"
;
#endif
if
(
transectsENU
.
size
()
==
0
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
"not able to generate transects."
;
#endif
return
PtrTransects
();
}
// Route transects;
std
::
vector
<
snake
::
TransectInfo
>
transectsInfo
;
snake
::
Route
route
;
std
::
string
errorString
;
bool
success
=
snake
::
route
(
polygonENU
,
transectsENU
,
transectsInfo
,
route
,
errorString
);
if
(
!
success
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
"routing failed."
;
#endif
return
PtrTransects
();
}
// Remove return path.
const
auto
&
info
=
transectsInfo
.
back
();
const
auto
&
lastTransect
=
transectsENU
[
info
.
index
];
const
auto
&
lastWaypoint
=
info
.
reversed
?
lastTransect
.
front
()
:
lastTransect
.
back
();
auto
&
wp
=
route
.
back
();
while
(
wp
!=
lastWaypoint
)
{
route
.
pop_back
();
wp
=
route
.
back
();
}
// Convert to geo coordinates.
QList
<
CoordInfo_t
>
transectList
;
transectList
.
reserve
(
route
.
size
());
for
(
const
auto
&
vertex
:
route
)
{
QGeoCoordinate
c
;
snake
::
fromENU
(
origin
,
vertex
,
c
);
CoordInfo_t
coordinfo
=
{
c
,
CoordTypeInterior
};
transectList
.
append
(
coordinfo
);
}
PtrTransects
pTransects
(
new
Transects
());
pTransects
->
append
(
transectList
);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
"concurrent update success."
;
#endif
return
pTransects
;
}
});
// QtConcurrent::run()
_watcher
.
setFuture
(
future
);
}
}
#ifdef DEBUG_CIRCULAR_SURVEY
this
->
_transects
.
append
(
list
);
else
{
// Mark transect as stored;
qWarning
()
this
->
_needsStoring
=
false
;
<<
"CircularSurvey::_rebuildTransectsPhase1(): preconditions failed."
;
}
else
if
(
this
->
_needsReversal
)
{
if
(
this
->
_transects
.
size
()
>
0
)
{
auto
&
t
=
this
->
_transects
.
front
();
QList
<
CoordInfo_t
>
list
;
list
.
reserve
(
t
.
size
());
for
(
auto
it
=
t
.
end
()
-
1
;
it
>=
t
.
begin
();
--
it
)
{
list
.
append
(
*
it
);
}
this
->
_transects
.
clear
();
this
->
_transects
.
append
(
list
);
}
}
#endif
this
->
_needsReversal
=
false
;
}
else
{
this
->
_transects
.
clear
();
auto
polygon
=
this
->
_surveyAreaPolygon
.
coordinateList
();
this
->
_pWorker
->
update
(
polygon
,
this
->
_referencePoint
,
this
->
_deltaR
.
rawValue
().
toDouble
()
*
bu
::
si
::
meter
,
this
->
_minLength
.
rawValue
().
toDouble
()
*
bu
::
si
::
meter
,
snake
::
Angle
(
this
->
_deltaAlpha
.
rawValue
().
toDouble
()
*
bu
::
degree
::
degree
));
}
}
}
}
...
@@ -590,40 +343,27 @@ void CircularSurvey::_recalcComplexDistance() {
...
@@ -590,40 +343,27 @@ void CircularSurvey::_recalcComplexDistance() {
// no cameraShots in Circular Survey, add if desired
// no cameraShots in Circular Survey, add if desired
void
CircularSurvey
::
_recalcCameraShots
()
{
_cameraShots
=
0
;
}
void
CircularSurvey
::
_recalcCameraShots
()
{
_cameraShots
=
0
;
}
void
CircularSurvey
::
_deferUpdate
()
{
void
CircularSurvey
::
_setTransects
(
CircularSurvey
::
PtrRoute
pRoute
)
{
if
(
!
_calculating
)
{
this
->
_pRoute
=
pRoute
;
#ifdef DEBUG_CIRCULAR_SURVEY
this
->
_needsStoring
=
true
;
qWarning
()
<<
"CircularSurvey::_deferUpdate(): defer update."
;
this
->
_rebuildTransects
();
#endif
_transectsDirty
=
true
;
if
(
_timer
.
isActive
())
{
_timer
.
stop
();
}
_timer
.
start
(
100
/*ms*/
);
}
else
{
_cancle
=
true
;
}
}
}
Fact
*
CircularSurvey
::
transectMinLength
()
{
return
&
_minLength
;
}
Fact
*
CircularSurvey
::
transectMinLength
()
{
return
&
_minLength
;
}
Fact
*
CircularSurvey
::
reverse
()
{
return
&
_reverse
;
}
bool
CircularSurvey
::
calculating
()
{
return
this
->
_pWorker
->
calculating
();
}
Fact
*
CircularSurvey
::
maxWaypoints
()
{
return
&
_maxWaypoints
;
}
bool
CircularSurvey
::
calculating
()
{
return
_calculating
;
}
/*!
/*!
\class CircularSurveyComplexItem
\class CircularSurveyComplexItem
\inmodule Wima
\inmodule Wima
\brief The \c CircularSurveyComplexItem class provides a survey mission
item
\brief The \c CircularSurveyComplexItem class provides a survey mission
with circular transects around a point of interest.
item
with circular transects around a point of interest.
CircularSurveyComplexItem class provides a survey mission item with
circular
CircularSurveyComplexItem class provides a survey mission item with
transects around a point of interest. Within the \c Wima module it's used to
circular transects around a point of interest. Within the \c Wima module
scan a defined area with constant angle (circular transects) to the base
it's used to scan a defined area with constant angle (circular transects)
station (point of interest).
to the base
station (point of interest).
\sa WimaArea
\sa WimaArea
*/
*/
src/Wima/CircularSurvey.h
View file @
6fd60bb5
...
@@ -6,9 +6,14 @@
...
@@ -6,9 +6,14 @@
#include "SettingsFact.h"
#include "SettingsFact.h"
#include "TransectStyleComplexItem.h"
#include "TransectStyleComplexItem.h"
class
CSWorker
;
class
CircularSurvey
:
public
TransectStyleComplexItem
{
class
CircularSurvey
:
public
TransectStyleComplexItem
{
Q_OBJECT
Q_OBJECT
public:
public:
using
Route
=
QList
<
QGeoCoordinate
>
;
using
PtrRoute
=
QSharedPointer
<
Route
>
;
/// @param vehicle Vehicle which this is being contructed for
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// use in the Plan View
...
@@ -16,19 +21,19 @@ public:
...
@@ -16,19 +21,19 @@ public:
/// polygon
/// polygon
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
QObject
*
parent
);
QObject
*
parent
);
~
CircularSurvey
();
Q_PROPERTY
(
QGeoCoordinate
refPoint
READ
refPoint
WRITE
setRefPoint
NOTIFY
Q_PROPERTY
(
QGeoCoordinate
refPoint
READ
refPoint
WRITE
setRefPoint
NOTIFY
refPointChanged
)
refPointChanged
)
Q_PROPERTY
(
Fact
*
deltaR
READ
deltaR
CONSTANT
)
Q_PROPERTY
(
Fact
*
deltaR
READ
deltaR
CONSTANT
)
Q_PROPERTY
(
Fact
*
deltaAlpha
READ
deltaAlpha
CONSTANT
)
Q_PROPERTY
(
Fact
*
deltaAlpha
READ
deltaAlpha
CONSTANT
)
Q_PROPERTY
(
Fact
*
transectMinLength
READ
transectMinLength
CONSTANT
)
Q_PROPERTY
(
Fact
*
transectMinLength
READ
transectMinLength
CONSTANT
)
Q_PROPERTY
(
Fact
*
reverse
READ
reverse
CONSTANT
)
Q_PROPERTY
(
Fact
*
maxWaypoints
READ
maxWaypoints
CONSTANT
)
Q_PROPERTY
(
bool
isInitialized
READ
isInitialized
WRITE
setIsInitialized
NOTIFY
Q_PROPERTY
(
bool
isInitialized
READ
isInitialized
WRITE
setIsInitialized
NOTIFY
isInitializedChanged
)
isInitializedChanged
)
Q_PROPERTY
(
bool
calculating
READ
calculating
NOTIFY
calculatingChanged
)
Q_PROPERTY
(
bool
calculating
READ
calculating
NOTIFY
calculatingChanged
)
Q_INVOKABLE
void
resetReference
(
void
);
Q_INVOKABLE
void
resetReference
(
void
);
Q_INVOKABLE
void
reverse
(
void
);
// Property setters
// Property setters
void
setRefPoint
(
const
QGeoCoordinate
&
refPt
);
void
setRefPoint
(
const
QGeoCoordinate
&
refPt
);
...
@@ -41,8 +46,6 @@ public:
...
@@ -41,8 +46,6 @@ public:
Fact
*
deltaR
();
Fact
*
deltaR
();
Fact
*
deltaAlpha
();
Fact
*
deltaAlpha
();
Fact
*
transectMinLength
();
Fact
*
transectMinLength
();
Fact
*
reverse
();
Fact
*
maxWaypoints
();
bool
calculating
();
bool
calculating
();
// Is true if survey was automatically generated, prevents initialisation from
// Is true if survey was automatically generated, prevents initialisation from
// gui.
// gui.
...
@@ -72,8 +75,6 @@ public:
...
@@ -72,8 +75,6 @@ public:
static
const
char
*
deltaRName
;
static
const
char
*
deltaRName
;
static
const
char
*
deltaAlphaName
;
static
const
char
*
deltaAlphaName
;
static
const
char
*
transectMinLengthName
;
static
const
char
*
transectMinLengthName
;
static
const
char
*
reverseName
;
static
const
char
*
maxWaypointsName
;
static
const
char
*
CircularSurveyName
;
static
const
char
*
CircularSurveyName
;
static
const
char
*
refPointLongitudeName
;
static
const
char
*
refPointLongitudeName
;
static
const
char
*
refPointLatitudeName
;
static
const
char
*
refPointLatitudeName
;
...
@@ -89,7 +90,7 @@ private slots:
...
@@ -89,7 +90,7 @@ private slots:
void
_rebuildTransectsPhase1
(
void
)
final
;
void
_rebuildTransectsPhase1
(
void
)
final
;
void
_recalcComplexDistance
(
void
)
final
;
void
_recalcComplexDistance
(
void
)
final
;
void
_recalcCameraShots
(
void
)
final
;
void
_recalcCameraShots
(
void
)
final
;
void
_
deferUpdate
(
);
void
_
setTransects
(
PtrRoute
pRoute
);
private:
private:
void
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
void
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
...
@@ -108,21 +109,14 @@ private:
...
@@ -108,21 +109,14 @@ private:
// minimal transect lenght, transects are rejected if they are shorter than
// minimal transect lenght, transects are rejected if they are shorter than
// this value
// this value
SettingsFact
_minLength
;
SettingsFact
_minLength
;
// reverses the _transects path
SettingsFact
_reverse
;
// the maximum number of waypoints _transects (TransectStyleComplexItem) can
// contain (to avoid performance hits)
SettingsFact
_maxWaypoints
;
// Timer to defer recalc
QTimer
_timer
;
// indicates if the polygon and refpoint etc. are initialized, prevents
// indicates if the polygon and refpoint etc. are initialized, prevents
// reinitialisation from gui and execution of _rebuildTransectsPhase1 during
// reinitialisation from gui and execution of _rebuildTransectsPhase1 during
// init from gui
// init from gui
bool
_isInitialized
;
bool
_isInitialized
;
using
Ptr
Transects
=
std
::
shared_ptr
<
Transects
>
;
using
Ptr
Worker
=
std
::
shared_ptr
<
CSWorker
>
;
using
Watcher
=
QFutureWatcher
<
PtrTransects
>
;
PtrWorker
_pWorker
;
Watcher
_watcher
;
PtrRoute
_pRoute
;
bool
_
calculat
ing
;
bool
_
needsStor
ing
;
bool
_
cancle
;
bool
_
needsReversal
;
};
};
src/Wima/Snake/snake.cpp
View file @
6fd60bb5
...
@@ -781,8 +781,8 @@ void generateRoutingModel(const BoostLineString &vertices,
...
@@ -781,8 +781,8 @@ void generateRoutingModel(const BoostLineString &vertices,
}
}
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
r
oute
,
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
r
,
string
&
errorString
)
{
st
d
::
function
<
bool
()
>
stop
,
st
ring
&
errorString
)
{
//=======================================
//=======================================
// Route Transects using Google or-tools.
// Route Transects using Google or-tools.
//=======================================
//=======================================
...
@@ -819,9 +819,17 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -819,9 +819,17 @@ bool route(const BoostPolygon &area, const Transects &transects,
vertices
.
push_back
(
vertex
);
vertices
.
push_back
(
vertex
);
}
}
size_t
n1
=
vertices
.
size
();
size_t
n1
=
vertices
.
size
();
if
(
stop
())
{
errorString
=
"User termination."
;
return
false
;
}
// Generate routing model.
// Generate routing model.
RoutingDataModel
dataModel
;
RoutingDataModel
dataModel
;
Matrix
<
double
>
connectionGraph
(
n1
,
n1
);
Matrix
<
double
>
connectionGraph
(
n1
,
n1
);
if
(
stop
())
{
errorString
=
"User termination."
;
return
false
;
}
// Offset joined area.
// Offset joined area.
BoostPolygon
areaOffset
;
BoostPolygon
areaOffset
;
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
...
@@ -835,6 +843,10 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -835,6 +843,10 @@ bool route(const BoostPolygon &area, const Transects &transects,
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
<<
endl
;
#endif
#endif
if
(
stop
())
{
errorString
=
"User termination."
;
return
false
;
}
// Create Routing Index Manager.
// Create Routing Index Manager.
RoutingIndexManager
manager
(
dataModel
.
distanceMatrix
.
getN
(),
RoutingIndexManager
manager
(
dataModel
.
distanceMatrix
.
getN
(),
...
@@ -883,16 +895,13 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -883,16 +895,13 @@ bool route(const BoostPolygon &area, const Transects &transects,
index
+=
1
;
index
+=
1
;
}
}
}
}
// Set first solution heuristic.
// Setting first solution heuristic.
auto
searchParameters
=
DefaultRoutingSearchParameters
();
RoutingSearchParameters
searchParameters
=
DefaultRoutingSearchParameters
();
searchParameters
.
set_first_solution_strategy
(
searchParameters
.
set_first_solution_strategy
(
FirstSolutionStrategy
::
PATH_CHEAPEST_ARC
);
FirstSolutionStrategy
::
PATH_CHEAPEST_ARC
);
google
::
protobuf
::
Duration
*
tMax
=
// Set costume limit.
new
google
::
protobuf
::
Duration
();
// seconds
auto
*
limit
=
solver
->
MakeCustomLimit
(
stop
);
tMax
->
set_seconds
(
10
);
routing
.
AddSearchMonitor
(
limit
);
searchParameters
.
set_allocated_time_limit
(
tMax
);
// Solve the problem.
// Solve the problem.
#ifdef SNAKE_SHOW_TIME
#ifdef SNAKE_SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
...
@@ -908,6 +917,9 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -908,6 +917,9 @@ bool route(const BoostPolygon &area, const Transects &transects,
if
(
!
solution
||
solution
->
Size
()
<=
1
)
{
if
(
!
solution
||
solution
->
Size
()
<=
1
)
{
errorString
=
"Not able to solve the routing problem."
;
errorString
=
"Not able to solve the routing problem."
;
return
false
;
return
false
;
}
else
if
(
stop
())
{
errorString
=
"User terminated."
;
return
false
;
}
}
// Extract index list from solution.
// Extract index list from solution.
...
@@ -944,12 +956,12 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -944,12 +956,12 @@ bool route(const BoostPolygon &area, const Transects &transects,
}
}
for
(
auto
it
=
reversedTransect
.
begin
();
for
(
auto
it
=
reversedTransect
.
begin
();
it
<
reversedTransect
.
end
()
-
1
;
++
it
)
{
it
<
reversedTransect
.
end
()
-
1
;
++
it
)
{
r
oute
.
push_back
(
*
it
);
r
.
push_back
(
*
it
);
}
}
}
else
{
}
else
{
const
auto
&
t
=
transects
[
info1
.
index
];
const
auto
&
t
=
transects
[
info1
.
index
];
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
)
{
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
)
{
r
oute
.
push_back
(
*
it
);
r
.
push_back
(
*
it
);
}
}
}
}
}
else
{
}
else
{
...
@@ -958,12 +970,19 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -958,12 +970,19 @@ bool route(const BoostPolygon &area, const Transects &transects,
if
(
i
!=
route_idx
.
size
()
-
2
)
{
if
(
i
!=
route_idx
.
size
()
-
2
)
{
idxList
.
pop_back
();
idxList
.
pop_back
();
}
}
idx2Vertex
(
idxList
,
r
oute
);
idx2Vertex
(
idxList
,
r
);
}
}
}
}
return
true
;
return
true
;
}
}
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
r
,
string
&
errorString
)
{
auto
stop
=
[]
{
return
false
;
};
return
route
(
area
,
transects
,
transectInfo
,
r
,
stop
,
errorString
);
}
}
// namespace snake
}
// namespace snake
bool
boost
::
geometry
::
model
::
operator
==
(
snake
::
BoostPoint
p1
,
bool
boost
::
geometry
::
model
::
operator
==
(
snake
::
BoostPoint
p1
,
...
...
src/Wima/Snake/snake.h
View file @
6fd60bb5
#pragma once
#pragma once
#include <array>
#include <array>
#include <atomic>
#include <functional>
#include <memory>
#include <memory>
#include <string>
#include <string>
#include <vector>
#include <vector>
...
@@ -185,8 +187,11 @@ struct TransectInfo {
...
@@ -185,8 +187,11 @@ struct TransectInfo {
bool
reversed
;
bool
reversed
;
};
};
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
r
oute
,
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
r
,
string
&
errorString
);
string
&
errorString
);
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
r
,
std
::
function
<
bool
(
void
)
>
stop
,
string
&
errorString
);
namespace
detail
{
namespace
detail
{
const
double
offsetConstant
=
const
double
offsetConstant
=
...
...
src/Wima/WimaPlaner.cc
View file @
6fd60bb5
...
@@ -21,12 +21,13 @@ const char *WimaPlaner::areaItemsName = "AreaItems";
...
@@ -21,12 +21,13 @@ const char *WimaPlaner::areaItemsName = "AreaItems";
const
char
*
WimaPlaner
::
missionItemsName
=
"MissionItems"
;
const
char
*
WimaPlaner
::
missionItemsName
=
"MissionItems"
;
WimaPlaner
::
WimaPlaner
(
QObject
*
parent
)
WimaPlaner
::
WimaPlaner
(
QObject
*
parent
)
:
QObject
(
parent
),
_currentAreaIndex
(
-
1
),
_wimaBridge
(
nullptr
),
:
QObject
(
parent
),
_masterController
(
nullptr
),
_missionController
(
nullptr
),
_joinedArea
(
this
),
_joinedAreaValid
(
false
),
_measurementArea
(
this
),
_currentAreaIndex
(
-
1
),
_wimaBridge
(
nullptr
),
_joinedArea
(
this
),
_serviceArea
(
this
),
_corridor
(
this
),
_TSComplexItem
(
nullptr
),
_joinedAreaValid
(
false
),
_arrivalPathLength
(
0
),
_returnPathLength
(
0
),
_surveyRefChanging
(
false
),
_measurementAreaChanging
(
false
),
_TSComplexItem
(
nullptr
),
_surveyRefChanging
(
false
),
_corridorChanging
(
false
),
_serviceAreaChanging
(
false
),
_measurementAreaChanging
(
false
),
_corridorChanging
(
false
),
_syncronizedWithController
(
false
),
_readyForSync
(
false
)
{
_serviceAreaChanging
(
false
),
_syncronizedWithController
(
false
),
_readyForSync
(
false
)
{
_lastMeasurementAreaPath
=
_measurementArea
.
path
();
_lastMeasurementAreaPath
=
_measurementArea
.
path
();
_lastServiceAreaPath
=
_serviceArea
.
path
();
_lastServiceAreaPath
=
_serviceArea
.
path
();
_lastCorridorPath
=
_corridor
.
path
();
_lastCorridorPath
=
_corridor
.
path
();
...
...
src/Wima/WimaPlaner.h
View file @
6fd60bb5
...
@@ -49,16 +49,16 @@ public:
...
@@ -49,16 +49,16 @@ public:
Q_PROPERTY
(
bool
readyForSync
READ
readyForSync
NOTIFY
readyForSyncChanged
)
Q_PROPERTY
(
bool
readyForSync
READ
readyForSync
NOTIFY
readyForSyncChanged
)
// Property accessors
// Property accessors
PlanMasterController
*
masterController
(
void
)
{
return
_masterController
;
}
PlanMasterController
*
masterController
(
void
)
;
MissionController
*
missionController
(
void
)
{
return
_missionController
;
}
MissionController
*
missionController
(
void
)
;
QmlObjectListModel
*
visualItems
(
void
);
QmlObjectListModel
*
visualItems
(
void
);
int
currentPolygonIndex
(
void
)
const
{
return
_currentAreaIndex
;
}
int
currentPolygonIndex
(
void
)
const
;
QString
currentFile
(
void
)
const
{
return
_currentFile
;
}
QString
currentFile
(
void
)
const
;
QStringList
loadNameFilters
(
void
)
const
;
QStringList
loadNameFilters
(
void
)
const
;
QStringList
saveNameFilters
(
void
)
const
;
QStringList
saveNameFilters
(
void
)
const
;
QString
fileExtension
(
void
)
const
{
return
wimaFileExtension
;
}
QString
fileExtension
(
void
)
const
;
QGeoCoordinate
joinedAreaCenter
(
void
)
const
;
QGeoCoordinate
joinedAreaCenter
(
void
)
const
;
WimaBridge
*
wimaBridge
(
void
)
{
return
_wimaBridge
;
}
WimaBridge
*
wimaBridge
(
void
)
;
// Property setters
// Property setters
void
setMasterController
(
PlanMasterController
*
masterController
);
void
setMasterController
(
PlanMasterController
*
masterController
);
...
@@ -145,22 +145,22 @@ private:
...
@@ -145,22 +145,22 @@ private:
PlanMasterController
*
_masterController
;
PlanMasterController
*
_masterController
;
MissionController
*
_missionController
;
MissionController
*
_missionController
;
int
_currentAreaIndex
;
int
_currentAreaIndex
;
QString
_currentFile
;
// file for saveing
QString
_currentFile
;
WimaBridge
*
_wimaBridge
;
// container for data exchange with WimaController
// container for data exchange with WimaController
QmlObjectListModel
_visualItems
;
// contains all visible areas
WimaBridge
*
_wimaBridge
;
WimaJoinedArea
_joinedArea
;
// joined area fromed by _measurementArea,
// _serviceArea, _corridor
bool
_joinedAreaValid
;
bool
_joinedAreaValid
;
WimaMeasurementArea
_measurementArea
;
// measurement area
WimaMeasurementArea
_measurementArea
;
WimaServiceArea
_serviceArea
;
// area for supplying
WimaServiceArea
_serviceArea
;
WimaCorridor
WimaCorridor
_corridor
;
_corridor
;
// corridor connecting _measurementArea and _serviceArea
// contains all visible areas
unsigned
long
QmlObjectListModel
_visualItems
;
_arrivalPathLength
;
// the number waypoints the arrival path consists of
// joined area fromed by _measurementArea, _serviceArea, _corridor
// (path from takeoff to first measurement point)
WimaJoinedArea
_joinedArea
;
unsigned
long
// path from takeoff to first measurement point
_returnPathLength
;
// the number waypoints the return path consists of
unsigned
long
_arrivalPathLength
;
// (path from last measurement point to land)
// path from last measurement point to land
unsigned
long
_returnPathLength
;
CircularSurvey
*
_TSComplexItem
;
// pointer to the CircularSurvey item in
CircularSurvey
*
_TSComplexItem
;
// pointer to the CircularSurvey item in
// _missionController.visualItems()
// _missionController.visualItems()
...
...
src/WimaView/CircularSurveyMapVisual.qml
View file @
6fd60bb5
...
@@ -7,8 +7,6 @@
...
@@ -7,8 +7,6 @@
*
*
****************************************************************************/
****************************************************************************/
// original file: SurveyMapVisual.qml
import
QtQuick
2.3
import
QtQuick
2.3
import
QtQuick
.
Controls
1.2
import
QtQuick
.
Controls
1.2
import
QtLocation
5.3
import
QtLocation
5.3
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment