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
f3a5cb44
Commit
f3a5cb44
authored
Aug 28, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
snake mod
parent
fbf79828
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
473 additions
and
524 deletions
+473
-524
snake.cpp
src/Snake/snake.cpp
+276
-353
snake.h
src/Snake/snake.h
+24
-57
snake_typedefs.h
src/Snake/snake_typedefs.h
+10
-2
SnakeDataManager.cc
src/Wima/Snake/SnakeDataManager.cc
+147
-109
SnakeDataManager.h
src/Wima/Snake/SnakeDataManager.h
+2
-1
WimaController.cc
src/Wima/WimaController.cc
+12
-0
WimaController.h
src/Wima/WimaController.h
+2
-2
No files found.
src/Snake/snake.cpp
View file @
f3a5cb44
...
...
@@ -7,6 +7,7 @@
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include "clipper/clipper.hpp"
...
...
@@ -26,13 +27,13 @@ using namespace operations_research;
namespace
bg
=
boost
::
geometry
;
namespace
trans
=
bg
::
strategy
::
transform
;
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS
(
bg
::
cs
::
cartesian
)
namespace
snake
{
//=========================================================================
// Geometry stuff.
//=========================================================================
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS
(
cs
::
cartesian
)
void
polygonCenter
(
const
BoostPolygon
&
polygon
,
BoostPoint
&
center
)
{
using
namespace
mapbox
;
...
...
@@ -385,9 +386,9 @@ void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex, size_
// Scenario calculation.
//=========================================================================
}
Scenario
::
Scenario
()
:
_tileWidth
(
5
)
,
_tileHeight
(
5
)
,
_minTileArea
(
0
)
_tileWidth
(
5
*
bu
::
si
::
meter
)
,
_tileHeight
(
5
*
bu
::
si
::
meter
)
,
_minTileArea
(
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
)
,
_needsUpdate
(
true
)
{
...
...
@@ -446,21 +447,6 @@ const BoostPolygon &Scenario::corridor() const
return
_corridor
;
}
BoostPolygon
&
Scenario
::
measurementArea
()
{
return
_mArea
;
}
BoostPolygon
&
Scenario
::
serviceArea
()
{
return
_sArea
;
}
BoostPolygon
&
Scenario
::
corridor
()
{
return
_corridor
;
}
const
BoostPolygon
&
Scenario
::
joinedArea
()
const
{
return
_jArea
;
}
...
...
@@ -521,13 +507,17 @@ bool Scenario::_calculateTiles()
_tiles
.
clear
();
_tileCenterPoints
.
clear
();
if
(
_tileWidth
<=
0
||
_tileHeight
<=
0
||
_minTileArea
<
0
)
{
if
(
_tileWidth
<=
0
*
bu
::
si
::
meter
||
_tileHeight
<=
0
*
bu
::
si
::
meter
||
_minTileArea
<=
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
)
{
errorString
=
"Parameters tileWidth, tileHeight, minTileArea must be positive."
;
return
false
;
}
double
bbox
_w
idth
=
_mAreaBoundingBox
.
width
;
double
bbox
_h
eight
=
_mAreaBoundingBox
.
height
;
double
bbox
W
idth
=
_mAreaBoundingBox
.
width
;
double
bbox
H
eight
=
_mAreaBoundingBox
.
height
;
BoostPoint
origin
=
_mAreaBoundingBox
.
corners
.
outer
()[
0
];
...
...
@@ -543,23 +533,27 @@ bool Scenario::_calculateTiles()
//cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t
i
_max
=
ceil
(
bbox_width
/
tileWidth
);
size_t
j
_max
=
ceil
(
bbox_height
/
tileHeight
);
size_t
i
Max
=
ceil
(
bboxWidth
/
_tileWidth
.
value
()
);
size_t
j
Max
=
ceil
(
bboxHeight
/
_tileHeight
.
value
()
);
if
(
i_max
<
1
||
j_max
<
1
)
{
errorString
=
"Tile width or Tile height to large."
;
if
(
iMax
<
1
||
jMax
<
1
)
{
std
::
stringstream
ss
;
ss
<<
"Tile width or Tile height to large. "
<<
"tile width = "
<<
_tileWidth
<<
", "
<<
"tile height = "
<<
_tileHeight
<<
"."
<<
std
::
endl
;
errorString
=
ss
.
str
();
return
false
;
}
trans
::
rotate_transformer
<
boost
::
geometry
::
degree
,
double
,
2
,
2
>
rotate_back
(
-
_mAreaBoundingBox
.
angle
*
180
/
M_PI
);
trans
::
translate_transformer
<
double
,
2
,
2
>
translate_back
(
origin
.
get
<
0
>
(),
origin
.
get
<
1
>
());
for
(
size_t
i
=
0
;
i
<
i
_m
ax
;
++
i
){
double
x_min
=
tileWidth
*
i
;
double
x_max
=
x_min
+
tileWidth
;
for
(
size_t
j
=
0
;
j
<
j
_m
ax
;
++
j
){
double
y_min
=
tileHeight
*
j
;
double
y_max
=
y_min
+
tileHeight
;
for
(
size_t
i
=
0
;
i
<
i
M
ax
;
++
i
){
double
x_min
=
_tileWidth
.
value
()
*
i
;
double
x_max
=
x_min
+
_tileWidth
.
value
()
;
for
(
size_t
j
=
0
;
j
<
j
M
ax
;
++
j
){
double
y_min
=
_tileHeight
.
value
()
*
j
;
double
y_max
=
y_min
+
_tileHeight
.
value
()
;
BoostPolygon
tile_unclipped
;
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_min
,
y_min
});
...
...
@@ -575,7 +569,7 @@ bool Scenario::_calculateTiles()
for
(
BoostPolygon
t
:
boost_tiles
)
{
if
(
bg
::
area
(
t
)
>
minTileArea
){
if
(
bg
::
area
(
t
)
>
_minTileArea
.
value
()
){
// Transform boost_tile to world coordinate system.
BoostPolygon
rotated_tile
;
BoostPolygon
translated_tile
;
...
...
@@ -612,9 +606,9 @@ bool Scenario::_calculateJoinedArea()
bool
corridor_is_connection
=
false
;
if
(
corridorValid
)
{
// Corridor overlaping with measurement area?
if
(
bg
::
intersects
(
_corrid
i
or
,
_mArea
)
)
{
if
(
bg
::
intersects
(
_corridor
,
_mArea
)
)
{
// Corridor overlaping with service area?
if
(
bg
::
intersects
(
_corrid
i
or
,
_sArea
)
)
{
if
(
bg
::
intersects
(
_corridor
,
_sArea
)
)
{
corridor_is_connection
=
true
;
}
}
...
...
@@ -625,10 +619,10 @@ bool Scenario::_calculateJoinedArea()
BoostPolygon
partialArea
=
_mArea
;
if
(
overlapingSerMeas
){
if
(
corridor_is_connection
){
bg
::
union_
(
partialArea
,
_corrid
i
or
,
sol
);
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
}
}
else
if
(
corridor_is_connection
){
bg
::
union_
(
partialArea
,
_corrid
i
or
,
sol
);
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
}
else
{
errorString
=
"Areas are not overlapping"
;
return
false
;
...
...
@@ -651,40 +645,40 @@ bool Scenario::_calculateJoinedArea()
return
true
;
}
double
Scenario
::
minTileArea
()
const
Area
Scenario
::
minTileArea
()
const
{
return
_minTileArea
;
}
void
Scenario
::
setMinTileArea
(
double
minTileArea
)
void
Scenario
::
setMinTileArea
(
Area
minTileArea
)
{
if
(
minTileArea
>=
0
){
if
(
minTileArea
>=
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
){
_needsUpdate
=
true
;
_minTileArea
=
minTileArea
;
}
}
double
Scenario
::
tileHeight
()
const
Length
Scenario
::
tileHeight
()
const
{
return
_tileHeight
;
}
void
Scenario
::
setTileHeight
(
double
tileHeight
)
void
Scenario
::
setTileHeight
(
Length
tileHeight
)
{
if
(
tileHeight
>
0
)
{
if
(
tileHeight
>
0
*
bu
::
si
::
meter
)
{
_needsUpdate
=
true
;
_tileHeight
=
tileHeight
;
}
}
double
Scenario
::
tileWidth
()
const
Length
Scenario
::
tileWidth
()
const
{
return
_tileWidth
;
}
void
Scenario
::
setTileWidth
(
double
tileWidth
)
void
Scenario
::
setTileWidth
(
Length
tileWidth
)
{
if
(
tileWidth
>
0
){
if
(
tileWidth
>
0
*
bu
::
si
::
meter
){
_needsUpdate
=
true
;
_tileWidth
=
tileWidth
;
}
...
...
@@ -710,7 +704,7 @@ bool joinAreas(const std::vector<BoostPolygon> &areas, BoostPolygon &joinedArea)
bg
::
union_
(
joinedArea
,
areas
[
*
it
],
sol
);
if
(
sol
.
size
()
>
0
)
{
joinedArea
=
sol
[
0
];
sol
.
clear
;
sol
.
clear
()
;
idxList
.
erase
(
it
);
success
=
true
;
break
;
...
...
@@ -739,112 +733,237 @@ void BoundingBox::clear()
corners
.
clear
();
}
Flightplan
::
Flightplan
(
ScenarioCPtr
s
,
ProgressCPtr
p
)
:
_scenario
(
s
)
,
_progress
(
p
)
bool
flight_plan
::
transectsFromScenario
(
Length
distance
,
Length
minLength
,
Angle
angle
,
const
Scenario
&
scenario
,
const
Progress
&
p
,
flight_plan
::
Transects
&
t
,
string
&
errorString
)
{
// Rotate measurement area by angle and calculate bounding box.
auto
&
mArea
=
scenario
.
measurementArea
();
BoostPolygon
mAreaRotated
;
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate
(
-
angle
.
value
()
*
180
/
M_PI
);
bg
::
transform
(
mArea
,
mAreaRotated
,
rotate
);
BoostBox
box
;
boost
::
geometry
::
envelope
(
mAreaRotated
,
box
);
double
x0
=
box
.
min_corner
().
get
<
0
>
();
double
y0
=
box
.
min_corner
().
get
<
1
>
();
double
x1
=
box
.
max_corner
().
get
<
0
>
();
double
y1
=
box
.
max_corner
().
get
<
1
>
();
// Generate transects and convert them to clipper path.
size_t
num_t
=
int
(
ceil
((
y1
-
y0
)
/
distance
.
value
()));
// number of transects
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate_back
(
angle
.
value
()
*
180
/
M_PI
);
vector
<
ClipperLib
::
Path
>
transectsClipper
;
transectsClipper
.
reserve
(
num_t
);
for
(
size_t
i
=
0
;
i
<
num_t
;
++
i
)
{
// calculate transect
BoostPoint
v1
{
x0
,
y0
+
i
*
distance
.
value
()};
BoostPoint
v2
{
x1
,
y0
+
i
*
distance
.
value
()};
BoostLineString
transect
;
transect
.
push_back
(
v1
);
transect
.
push_back
(
v2
);
}
// transform back
BoostLineString
temp_transect
;
bg
::
transform
(
transect
,
temp_transect
,
rotate_back
);
double
Flightplan
::
lineDistance
()
const
{
return
_lineDistance
;
}
void
Flightplan
::
setLineDistance
(
double
lineDistance
)
{
_lineDistance
=
lineDistance
;
}
ClipperLib
::
IntPoint
c1
{
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
ClipperLib
::
IntPoint
c2
{
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
ClipperLib
::
Path
path
{
c1
,
c2
};
transectsClipper
.
push_back
(
path
);
}
double
Flightplan
::
minTransectLength
()
const
{
return
_minTransectLength
;
}
if
(
transectsClipper
.
size
()
==
0
){
std
::
stringstream
ss
;
ss
<<
"Not able to generate transects. Parameter: distance = "
<<
distance
<<
std
::
endl
;
errorString
=
ss
.
str
();
return
false
;
}
void
Flightplan
::
setMinTransectLength
(
double
minTransectLength
)
{
_minTransectLength
=
minTransectLength
;
}
// Convert measurement area to clipper path.
ClipperLib
::
Path
mAreaClipper
;
for
(
auto
vertex
:
mArea
.
outer
()
){
mAreaClipper
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
}
Flightplan
::
ScenarioCPtr
Flightplan
::
scenario
()
const
{
return
_scenario
;
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib
::
Clipper
clipper
;
clipper
.
AddPath
(
mAreaClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
AddPaths
(
transectsClipper
,
ClipperLib
::
ptSubject
,
false
);
ClipperLib
::
PolyTree
clippedTransecs
;
clipper
.
Execute
(
ClipperLib
::
ctIntersection
,
clippedTransecs
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
auto
&
transects
=
clippedTransecs
;
bool
ignoreProgress
=
p
.
size
()
!=
scenario
.
tiles
().
size
();
ClipperLib
::
PolyTree
clippedTransecs2
;
if
(
!
ignoreProgress
)
{
// Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
size_t
numTiles
=
p
.
size
();
vector
<
BoostPolygon
>
processedTiles
;
const
auto
&
tiles
=
scenario
.
tiles
();
for
(
size_t
i
=
0
;
i
<
numTiles
;
++
i
)
{
if
(
p
[
i
]
==
100
){
processedTiles
.
push_back
(
tiles
[
i
]);
}
}
void
Flightplan
::
setScenario
(
ScenarioCPtr
&
scenario
)
{
_scenario
=
scenario
;
}
if
(
processedTiles
.
size
()
!=
numTiles
){
vector
<
ClipperLib
::
Path
>
processedTilesClipper
;
for
(
auto
t
:
processedTiles
){
ClipperLib
::
Path
path
;
for
(
auto
vertex
:
t
.
outer
()){
path
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
}
processedTilesClipper
.
push_back
(
path
);
}
Flightplan
::
ProgressCPtr
Flightplan
::
progress
()
const
{
return
_progress
;
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper
.
Clear
();
for
(
auto
&
child
:
clippedTransecs
.
Childs
)
clipper
.
AddPath
(
child
->
Contour
,
ClipperLib
::
ptSubject
,
false
);
clipper
.
AddPaths
(
processedTilesClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
Execute
(
ClipperLib
::
ctDifference
,
clippedTransecs2
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
transects
=
clippedTransecs2
;
}
}
void
Flightplan
::
setProgress
(
ProgressCPtr
&
progress
)
{
_progress
=
progress
;
// Extract transects from PolyTree and convert them to BoostLineString
for
(
auto
&
child
:
transects
.
Childs
){
auto
&
clipperTransect
=
child
->
Contour
;
BoostPoint
v1
{
static_cast
<
double
>
(
clipperTransect
[
0
].
X
)
/
CLIPPER_SCALE
,
static_cast
<
double
>
(
clipperTransect
[
0
].
Y
)
/
CLIPPER_SCALE
};
BoostPoint
v2
{
static_cast
<
double
>
(
clipperTransect
[
1
].
X
)
/
CLIPPER_SCALE
,
static_cast
<
double
>
(
clipperTransect
[
1
].
Y
)
/
CLIPPER_SCALE
};
BoostLineString
transect
{
v1
,
v2
};
if
(
bg
::
length
(
transect
)
>=
minLength
.
value
())
t
.
push_back
(
transect
);
}
if
(
t
.
size
()
==
0
){
std
::
stringstream
ss
;
ss
<<
"Not able to generate transects. Parameter: minLength = "
<<
minLength
<<
std
::
endl
;
errorString
=
ss
.
str
();
return
false
;
}
return
true
;
}
struct
Flightplan
::
RoutingDataModel
{
struct
RoutingDataModel
{
Matrix
<
int64_t
>
distanceMatrix
;
long
numVehicles
;
RoutingIndexManager
::
NodeIndex
depot
;
};
bool
Flightplan
::
update
()
{
_waypoints
.
clear
();
_arrivalPath
.
clear
();
_returnPath
.
clear
();
void
generateRoutingModel
(
const
BoostLineString
&
vertices
,
const
BoostPolygon
&
polygonOffset
,
size_t
n0
,
RoutingDataModel
&
dataModel
,
Matrix
<
double
>
&
graph
){
#ifdef SHOW_TIME
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
if
(
!
_generateTransects
())
return
false
;
graphFromPolygon
(
polygonOffset
,
vertices
,
graph
);
#ifdef SHOW_TIME
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time graphFromPolygon(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
Matrix
<
double
>
distanceMatrix
(
graph
);
#ifdef SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
toDistanceMatrix
(
distanceMatrix
);
#ifdef SHOW_TIME
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
endl
;
cout
<<
"Execution time _generateTransects(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time toDistanceMatrix(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
dataModel
.
distanceMatrix
.
setDimension
(
n0
,
n0
);
for
(
size_t
i
=
0
;
i
<
n0
;
++
i
){
dataModel
.
distanceMatrix
.
set
(
i
,
i
,
0
);
for
(
size_t
j
=
i
+
1
;
j
<
n0
;
++
j
){
dataModel
.
distanceMatrix
.
set
(
i
,
j
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
dataModel
.
distanceMatrix
.
set
(
j
,
i
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
}
}
dataModel
.
numVehicles
=
1
;
dataModel
.
depot
=
0
;
}
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.
//=======================================
// Offset joined area.
const
BoostPolygon
&
jArea
=
_scenario
->
joinedArea
();
BoostPolygon
jAreaOffset
;
offsetPolygon
(
jArea
,
jAreaOffset
,
detail
::
offsetConstant
);
// Create vertex list;
BoostLineString
vertices
;
size_t
n_t
=
_transects
.
size
()
*
2
;
size_t
n0
=
n_t
+
1
;
size_t
n0
=
0
;
for
(
const
auto
&
t
:
transects
){
if
(
t
.
size
()
>=
2
){
n0
+=
2
;
}
else
{
n0
+=
1
;
}
}
vertices
.
reserve
(
n0
);
for
(
auto
&
lstring
:
_transects
){
for
(
auto
&
vertex
:
lstring
){
vertices
.
push_back
(
vertex
);
struct
TransectInfo
{
TransectInfo
(
size_t
n
,
bool
f
)
:
index
(
n
)
,
front
(
f
)
{}
size_t
index
;
bool
front
;
};
std
::
vector
<
TransectInfo
>
transectInfoList
;
size_t
idx
=
0
;
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
});
}
}
vertices
.
push_back
(
_scenario
->
homePositon
());
for
(
long
i
=
0
;
i
<
long
(
jA
rea
.
outer
().
size
())
-
1
;
++
i
)
{
vertices
.
push_back
(
jA
rea
.
outer
()[
i
]);
for
(
long
i
=
0
;
i
<
long
(
a
rea
.
outer
().
size
())
-
1
;
++
i
)
{
vertices
.
push_back
(
a
rea
.
outer
()[
i
]);
}
for
(
auto
ring
:
jA
rea
.
inners
())
{
for
(
auto
vertex
:
ring
)
for
(
auto
&
ring
:
a
rea
.
inners
())
{
for
(
auto
&
vertex
:
ring
)
vertices
.
push_back
(
vertex
);
}
size_t
n1
=
vertices
.
size
();
// Generate routing model.
RoutingDataModel
dataModel
;
Matrix
<
double
>
connectionGraph
(
n1
,
n1
);
Matrix
<
double
>
connectionGraph
(
n1
,
n1
);
// Offset joined area.
BoostPolygon
areaOffset
;
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
#ifdef SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
_generateRoutingModel
(
vertices
,
jA
reaOffset
,
n0
,
dataModel
,
connectionGraph
);
generateRoutingModel
(
vertices
,
a
reaOffset
,
n0
,
dataModel
,
connectionGraph
);
#ifdef SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
...
...
@@ -857,7 +976,7 @@ bool Flightplan::update()
RoutingModel
routing
(
manager
);
// Create and register a transit callback.
const
int
transit
_callback_i
ndex
=
routing
.
RegisterTransitCallback
(
const
int
transit
CallbackI
ndex
=
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
();
...
...
@@ -866,32 +985,30 @@ bool Flightplan::update()
});
// Define cost of each arc.
routing
.
SetArcCostEvaluatorOfAllVehicles
(
transit
_callback_i
ndex
);
routing
.
SetArcCostEvaluatorOfAllVehicles
(
transit
CallbackI
ndex
);
// Define Constraints
.
size_t
n
=
_transects
.
size
()
*
2
;
// Define Constraints
(this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
Solver
*
solver
=
routing
.
solver
();
for
(
size_t
i
=
0
;
i
<
n
;
i
=
i
+
2
){
// auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
// auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
// auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
// auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
// auto c = solver->MakeNonEquality(cond0, cond1);
// solver->AddConstraint(c);
// alternative
auto
idx0
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
i
));
auto
idx1
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
i
+
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
();
solver
->
AddConstraint
(
c
);
size_t
index
=
0
;
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
){
const
auto
&
t
=
transects
[
i
];
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
();
solver
->
AddConstraint
(
c
);
index
+=
2
;
}
else
{
index
+=
1
;
}
}
// Setting first solution heuristic.
RoutingSearchParameters
searchParameters
=
DefaultRoutingSearchParameters
();
searchParameters
.
set_first_solution_strategy
(
...
...
@@ -915,236 +1032,42 @@ bool Flightplan::update()
return
false
;
}
// Extract
waypoints
from solution.
long
index
=
routing
.
Start
(
0
);
std
::
vector
<
size_t
>
route
;
route
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
// 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
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
route
_idx
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
}
long
sz
=
route
.
size
();
// Helper Lambda.
auto
fromVertices
=
[
&
vertices
](
const
std
::
vector
<
size_t
>
&
idxArray
,
auto
idx2Vertex
=
[
&
vertices
](
const
std
::
vector
<
size_t
>
&
idxArray
,
std
::
vector
<
BoostPoint
>
&
path
){
for
(
size_t
j
=
1
;
j
<
idxArray
.
size
();
++
j
)
path
.
push_back
(
vertices
[
idx
Array
[
j
]
]);
for
(
auto
idx
:
idxArray
)
path
.
push_back
(
vertices
[
idx
]);
};
// Fill arrival path.
_arrivalPath
.
push_back
(
vertices
[
route
[
0
]]);
size_t
idx0
=
route
[
0
];
size_t
idx1
=
route
[
1
];
std
::
vector
<
size_t
>
pathIdx
;
shortestPathFromGraph
(
connectionGraph
,
idx0
,
idx1
,
pathIdx
);
fromVertices
(
pathIdx
,
_arrivalPath
);
if
(
_arrivalPath
.
size
()
<
2
)
return
false
;
// Fill waypoints.
_waypoints
.
push_back
(
vertices
[
route
[
1
]]);
for
(
long
i
=
1
;
i
<
sz
-
2
;
++
i
){
size_t
idx0
=
route
[
i
];
size_t
idx1
=
route
[
i
+
1
];
pathIdx
.
clear
();
shortestPathFromGraph
(
connectionGraph
,
idx0
,
idx1
,
pathIdx
);
fromVertices
(
pathIdx
,
_waypoints
);
}
_waypoints
.
push_back
(
vertices
[
route
[
sz
-
2
]]);
if
(
_waypoints
.
size
()
<
2
)
return
false
;
// Fill return path.
_returnPath
.
push_back
(
vertices
[
route
[
sz
-
2
]]);
idx0
=
route
[
sz
-
2
];
idx1
=
route
[
sz
-
1
];
pathIdx
.
clear
();
shortestPathFromGraph
(
connectionGraph
,
idx0
,
idx1
,
pathIdx
);
fromVertices
(
pathIdx
,
_returnPath
);
if
(
_returnPath
.
size
()
<
2
)
return
false
;
return
true
;
}
bool
Flightplan
::
_generateTransects
()
{
_transects
.
clear
();
if
(
_scenario
->
tiles
().
size
()
!=
_progress
->
size
()){
ostringstream
strstream
;
strstream
<<
"Number of tiles ("
<<
_scenario
->
tiles
().
size
()
<<
") is not equal to progress array length ("
<<
_progress
->
size
()
<<
")"
;
errorString
=
strstream
.
str
();
return
false
;
}
// Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
size_t
num_tiles
=
_progress
->
size
();
vector
<
BoostPolygon
>
processedTiles
;
{
const
auto
&
tiles
=
_scenario
->
tiles
();
for
(
size_t
i
=
0
;
i
<
num_tiles
;
++
i
)
{
if
(
_progress
[
i
]
==
100
){
processedTiles
.
push_back
(
tiles
[
i
]);
// 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
[
idx0
];
if
(
info
.
front
){
const
auto
&
t
=
transects
[
info
.
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
);
idxList
.
pop_back
();
idx2Vertex
(
idxList
,
route
);
}
if
(
processedTiles
.
size
()
==
num_tiles
)
return
true
;
}
// Convert measurement area and tiles to clipper path.
ClipperLib
::
Path
mAreaClipper
;
for
(
auto
vertex
:
_scenario
->
measurementArea
().
outer
()
){
mAreaClipper
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
}
vector
<
ClipperLib
::
Path
>
processedTilesClipper
;
for
(
auto
t
:
processedTiles
){
ClipperLib
::
Path
path
;
for
(
auto
vertex
:
t
.
outer
()){
path
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
}
processedTilesClipper
.
push_back
(
path
);
}
const
min_bbox_rt
&
bbox
=
_scenario
->
mAreaBoundingBox
();
double
alpha
=
bbox
.
angle
;
double
x0
=
bbox
.
corners
.
outer
()[
0
].
get
<
0
>
();
double
y0
=
bbox
.
corners
.
outer
()[
0
].
get
<
1
>
();
double
bboxWidth
=
bbox
.
width
;
double
bboxHeight
=
bbox
.
height
;
double
delta
=
detail
::
offsetConstant
;
size_t
num_t
=
int
(
ceil
((
bboxHeight
+
2
*
delta
)
/
_lineDistance
));
// number of transects
vector
<
double
>
yCoords
;
yCoords
.
reserve
(
num_t
);
double
y
=
-
delta
;
for
(
size_t
i
=
0
;
i
<
num_t
;
++
i
)
{
yCoords
.
push_back
(
y
);
y
+=
_lineDistance
;
}
// Generate transects and convert them to clipper path.
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate_back
(
-
alpha
*
180
/
M_PI
);
trans
::
translate_transformer
<
double
,
2
,
2
>
translate_back
(
x0
,
y0
);
vector
<
ClipperLib
::
Path
>
transectsClipper
;
transectsClipper
.
reserve
(
num_t
);
for
(
size_t
i
=
0
;
i
<
num_t
;
++
i
)
{
// calculate transect
BoostPoint
v1
{
-
delta
,
yCoords
[
i
]};
BoostPoint
v2
{
bboxWidth
+
delta
,
yCoords
[
i
]};
BoostLineString
transect
;
transect
.
push_back
(
v1
);
transect
.
push_back
(
v2
);
// transform back
BoostLineString
temp_transect
;
bg
::
transform
(
transect
,
temp_transect
,
rotate_back
);
transect
.
clear
();
bg
::
transform
(
temp_transect
,
transect
,
translate_back
);
ClipperLib
::
IntPoint
c1
{
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
ClipperLib
::
IntPoint
c2
{
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
ClipperLib
::
Path
path
{
c1
,
c2
};
transectsClipper
.
push_back
(
path
);
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib
::
Clipper
clipper
;
clipper
.
AddPath
(
mAreaClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
AddPaths
(
transectsClipper
,
ClipperLib
::
ptSubject
,
false
);
ClipperLib
::
PolyTree
clippedTransecsPolyTree1
;
clipper
.
Execute
(
ClipperLib
::
ctIntersection
,
clippedTransecsPolyTree1
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper
.
Clear
();
for
(
auto
child
:
clippedTransecsPolyTree1
.
Childs
)
clipper
.
AddPath
(
child
->
Contour
,
ClipperLib
::
ptSubject
,
false
);
clipper
.
AddPaths
(
processedTilesClipper
,
ClipperLib
::
ptClip
,
true
);
ClipperLib
::
PolyTree
clippedTransecsPolyTree2
;
clipper
.
Execute
(
ClipperLib
::
ctDifference
,
clippedTransecsPolyTree2
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
// Extract transects from PolyTree and convert them to BoostLineString
for
(
auto
child
:
clippedTransecsPolyTree2
.
Childs
){
ClipperLib
::
Path
clipperTransect
=
child
->
Contour
;
BoostPoint
v1
{
static_cast
<
double
>
(
clipperTransect
[
0
].
X
)
/
CLIPPER_SCALE
,
static_cast
<
double
>
(
clipperTransect
[
0
].
Y
)
/
CLIPPER_SCALE
};
BoostPoint
v2
{
static_cast
<
double
>
(
clipperTransect
[
1
].
X
)
/
CLIPPER_SCALE
,
static_cast
<
double
>
(
clipperTransect
[
1
].
Y
)
/
CLIPPER_SCALE
};
BoostLineString
transect
{
v1
,
v2
};
if
(
bg
::
length
(
transect
)
>=
_minTransectLength
)
_transects
.
push_back
(
transect
);
}
if
(
_transects
.
size
()
<
1
)
return
false
;
return
true
;
}
void
Flightplan
::
_generateRoutingModel
(
const
BoostLineString
&
vertices
,
const
BoostPolygon
&
polygonOffset
,
size_t
n0
,
Flightplan
::
RoutingDataModel
&
dataModel
,
Matrix
<
double
>
&
graph
)
{
#ifdef SHOW_TIME
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
graphFromPolygon
(
polygonOffset
,
vertices
,
graph
);
#ifdef SHOW_TIME
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time graphFromPolygon(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
// cout << endl;
// for (size_t i=0; i<graph.size(); ++i){
// vector<double> &row = graph[i];
// for (size_t j=0; j<row.size();++j){
// cout << "(" << i << "," << j << "):" << row[j] << " ";
// }
// cout << endl;
// }
// cout << endl;
Matrix
<
double
>
distanceMatrix
(
graph
);
#ifdef SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
toDistanceMatrix
(
distanceMatrix
);
#ifdef SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time toDistanceMatrix(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
dataModel
.
distanceMatrix
.
setDimension
(
n0
,
n0
);
for
(
size_t
i
=
0
;
i
<
n0
;
++
i
){
dataModel
.
distanceMatrix
.
set
(
i
,
i
,
0
);
for
(
size_t
j
=
i
+
1
;
j
<
n0
;
++
j
){
dataModel
.
distanceMatrix
.
set
(
i
,
j
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
dataModel
.
distanceMatrix
.
set
(
j
,
i
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
}
}
dataModel
.
numVehicles
=
1
;
dataModel
.
depot
=
n0
-
1
;
}
}
}
// namespace snake
src/Snake/snake.h
View file @
f3a5cb44
...
...
@@ -158,10 +158,6 @@ class Scenario{
void
setServiceArea
(
const
BoostPolygon
&
area
);
void
setCorridor
(
const
BoostPolygon
&
area
);
BoostPolygon
&
measurementArea
();
BoostPolygon
&
serviceArea
();
BoostPolygon
&
corridor
();
const
BoundingBox
&
mAreaBoundingBox
()
const
;
const
BoostPolygon
&
measurementArea
()
const
;
...
...
@@ -242,60 +238,31 @@ bool joinAreas(const std::vector<BoostPolygon> &areas,
BoostPolygon
&
joinedArea
);
//========================================================================================
//
Flight
plan
//
flight_
plan
//========================================================================================
class
Flightplan
{
public:
using
ScenarioPtr
=
shared_ptr
<
Scenario
>
;
using
ProgressPtr
=
shared_ptr
<
vector
<
int
>>
;
Flightplan
(
ScenarioPtr
s
,
ScenarioPtr
p
);
const
vector
<
BoostPoint
>
&
waypoints
(
void
)
const
{
return
_waypoints
;}
const
vector
<
BoostPoint
>
&
arrivalPath
(
void
)
const
{
return
_arrivalPath
;}
const
vector
<
BoostPoint
>
&
returnPath
(
void
)
const
{
return
_returnPath
;}
double
lineDistance
()
const
;
void
setLineDistance
(
double
lineDistance
);
double
minTransectLengthconst
;
void
setMinTransectLength
(
double
minTransectLength
);
ScenarioPtr
scenario
()
const
;
void
setScenario
(
ScenarioPtr
&
scenario
);
ProgressPtr
progress
()
const
;
void
setProgress
(
ProgressPtr
&
progress
);
bool
update
();
string
errorString
;
private:
// Search Filter to speed up routing.SolveWithParameters(...);
// Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
class
SearchFilter
;
struct
RoutingDataModel
;
bool
_generateTransects
();
void
_generateRoutingModel
(
const
BoostLineString
&
vertices
,
const
BoostPolygon
&
polygonOffset
,
size_t
n0
,
RoutingDataModel
&
dataModel
,
Matrix
<
double
>
&
graph
);
double
_lineDistance
;
double
_minTransectLength
;
shared_ptr
<
const
Scenario
>
_pScenario
;
shared_ptr
<
const
vector
<
int
>>
_pProgress
;
vector
<
BoostPoint
>
_waypoints
;
vector
<
BoostPoint
>
_arrivalPath
;
vector
<
BoostPoint
>
_returnPath
;
vector
<
BoostLineString
>
_transects
;
};
namespace
detail
{
const
double
offsetConstant
=
0
.
1
;
// meter, polygon offset to compenstate for numerical inaccurracies.
}
namespace
flight_plan
{
using
Transects
=
vector
<
BoostLineString
>
;
using
Progress
=
vector
<
int
>
;
using
Route
=
vector
<
BoostPoint
>
;
bool
transectsFromScenario
(
Length
distance
,
Length
minLength
,
Angle
angle
,
const
Scenario
&
scenario
,
const
Progress
&
p
,
Transects
&
t
,
string
&
errorString
);
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
Transects
&
transectsRouted
,
Route
&
route
,
string
&
errorString
);
}
// namespace flight_plan
namespace
detail
{
const
double
offsetConstant
=
0
.
1
;
// meter, polygon offset to compenstate for numerical inaccurracies.
}
// namespace detail
}
src/Snake/snake_typedefs.h
View file @
f3a5cb44
...
...
@@ -3,7 +3,13 @@
#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
;
...
...
@@ -14,8 +20,10 @@ typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
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
::
length
,
double
>
Length
;
typedef
bu
::
quantity
<
bu
::
si
::
area
,
double
>
Area
;
typedef
bu
::
quantity
<
bu
::
si
::
plane_angle
,
double
>
Angle
;
}
src/Wima/Snake/SnakeDataManager.cc
View file @
f3a5cb44
...
...
@@ -66,7 +66,8 @@ public:
// Output
std
::
atomic_bool
calcInProgress
;
QNemoProgress
progress
;
QNemoProgress
qProgress
;
std
::
vector
<
int
>
progress
;
QNemoHeartbeat
heartbeat
;
QVector
<
QGeoCoordinate
>
waypoints
;
...
...
@@ -106,7 +107,7 @@ private:
SnakeDataManager
::
SnakeDataManager
(
QObject
*
parent
)
:
QThread
(
parent
)
,
_
pImpl
(
std
::
make_unique
<
SnakeImpl
>
())
,
pImpl
(
std
::
make_unique
<
SnakeImpl
>
())
{
}
...
...
@@ -118,85 +119,85 @@ SnakeDataManager::~SnakeDataManager()
void
SnakeDataManager
::
setMeasurementArea
(
const
QList
<
QGeoCoordinate
>
&
measurementArea
)
{
UniqueLock
lk
(
this
->
_pImpl
->
m
);
this
->
_
pImpl
->
mArea
=
measurementArea
;
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
mArea
=
measurementArea
;
}
void
SnakeDataManager
::
setServiceArea
(
const
QList
<
QGeoCoordinate
>
&
serviceArea
)
{
UniqueLock
lk
(
this
->
_pImpl
->
m
);
this
->
_
pImpl
->
sArea
=
serviceArea
;
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
sArea
=
serviceArea
;
}
void
SnakeDataManager
::
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
)
{
UniqueLock
lk
(
this
->
_pImpl
->
m
);
this
->
_
pImpl
->
corridor
=
corridor
;
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
corridor
=
corridor
;
}
QNemoProgress
SnakeDataManager
::
progress
()
{
SharedLock
lk
(
this
->
_pImpl
->
m
);
return
this
->
_pImpl
->
p
rogress
;
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
qP
rogress
;
}
QNemoHeartbeat
SnakeDataManager
::
heartbeat
()
{
SharedLock
lk
(
this
->
_pImpl
->
m
);
return
this
->
_
pImpl
->
heartbeat
;
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
heartbeat
;
}
bool
SnakeDataManager
::
calcInProgress
()
{
return
this
->
_
pImpl
->
calcInProgress
.
load
();
return
this
->
pImpl
->
calcInProgress
.
load
();
}
Length
SnakeDataManager
::
lineDistance
()
const
{
SharedLock
lk
(
this
->
_pImpl
->
m
);
return
this
->
_
pImpl
->
lineDistance
;
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
lineDistance
;
}
void
SnakeDataManager
::
setLineDistance
(
Length
lineDistance
)
{
UniqueLock
lk
(
this
->
_pImpl
->
m
);
this
->
_
pImpl
->
lineDistance
=
lineDistance
;
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
lineDistance
=
lineDistance
;
}
Area
SnakeDataManager
::
minTileArea
()
const
{
SharedLock
lk
(
this
->
_pImpl
->
m
);
return
this
->
_
pImpl
->
scenario
.
minTileArea
();
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
scenario
.
minTileArea
();
}
void
SnakeDataManager
::
setMinTileArea
(
Area
minTileArea
)
{
UniqueLock
lk
(
this
->
_pImpl
->
m
);
this
->
_
pImpl
->
scenario
.
setMinTileArea
(
minTileArea
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
scenario
.
setMinTileArea
(
minTileArea
);
}
Length
SnakeDataManager
::
tileHeight
()
const
{
SharedLock
lk
(
this
->
_pImpl
->
m
);
return
this
->
_
pImpl
->
scenario
.
tileHeight
();
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
scenario
.
tileHeight
();
}
void
SnakeDataManager
::
setTileHeight
(
Length
tileHeight
)
{
UniqueLock
lk
(
this
->
_pImpl
->
m
);
this
->
_
pImpl
->
scenario
.
setTileHeight
(
tileHeight
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
scenario
.
setTileHeight
(
tileHeight
);
}
Length
SnakeDataManager
::
tileWidth
()
const
{
SharedLock
lk
(
this
->
_pImpl
->
m
);
return
this
->
_
pImpl
->
scenario
.
tileWidth
();
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
scenario
.
tileWidth
();
}
void
SnakeDataManager
::
setTileWidth
(
Length
tileWidth
)
{
UniqueLock
lk
(
this
->
_pImpl
->
m
);
this
->
_
pImpl
->
scenario
.
setTileWidth
(
tileWidth
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
scenario
.
setTileWidth
(
tileWidth
);
}
bool
SnakeDataManager
::
precondition
()
const
...
...
@@ -204,81 +205,143 @@ bool SnakeDataManager::precondition() const
return
true
;
}
//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void
SnakeDataManager
::
resetWaypointData
()
{
this
->
pImpl
->
waypoints
.
clear
();
this
->
pImpl
->
arrivalPath
.
clear
();
this
->
pImpl
->
returnPath
.
clear
();
this
->
pImpl
->
ENUorigin
=
QGeoCoordinate
(
0.0
,
0.0
,
0.0
);
this
->
pImpl
->
waypointsENU
.
clear
();
this
->
pImpl
->
arrivalPathENU
.
clear
();
this
->
pImpl
->
returnPathENU
.
clear
();
this
->
pImpl
->
tiles
.
polygons
().
clear
();
this
->
pImpl
->
tileCenterPoints
.
clear
();
this
->
pImpl
->
tilesENU
.
polygons
().
clear
();
this
->
pImpl
->
tileCenterPointsENU
.
clear
();
this
->
pImpl
->
errorMessage
.
clear
();
}
void
SnakeDataManager
::
run
()
{
this
->
_
pImpl
->
calcInProgress
.
store
(
true
);
ToggleRAII
<
std
::
atomic_bool
>
tr
(
this
->
_
pImpl
->
calcInProgress
);
this
->
pImpl
->
calcInProgress
.
store
(
true
);
ToggleRAII
<
std
::
atomic_bool
>
tr
(
this
->
pImpl
->
calcInProgress
);
UniqueLock
lk
(
&
_pImpl
->
m
);
_pImpl
->
clear
();
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
resetWaypointData
();
if
(
!
precondition
()
)
return
;
if
(
this
->
_
pImpl
->
mArea
.
size
()
<
3
)
{
_
pImpl
->
errorMessage
=
"Measurement area invalid: size < 3."
;
if
(
this
->
pImpl
->
mArea
.
size
()
<
3
)
{
pImpl
->
errorMessage
=
"Measurement area invalid: size < 3."
;
return
;
}
if
(
this
->
_
pImpl
->
sArea
.
size
()
<
3
)
{
_
pImpl
->
errorMessage
=
"Service area invalid: size < 3."
;
if
(
this
->
pImpl
->
sArea
.
size
()
<
3
)
{
pImpl
->
errorMessage
=
"Service area invalid: size < 3."
;
return
;
}
||
_sArea
.
size
()
<=
0
||
_corridor
.
size
()
<=
0
){
auto
origin
=
_mArea
.
front
();
// Measurement area update necessary.
if
(
_mArea
.
size
()
!=
_pScenario
->
measurementArea
().
outer
().
size
()
){
{
QMutexLocker
lk
(
&
_pImpl
->
m
);
_pImpl
->
ENUorigin
=
origin
;
}
for
(
auto
geoVertex
:
_mArea
){
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
_pScenario
->
measurementArea
().
outer
().
push_back
(
p
);
}
this
->
pImpl
->
ENUorigin
=
this
->
pImpl
->
mArea
.
front
();
auto
&
origin
=
this
->
pImpl
->
ENUorigin
;
// Update measurement area.
auto
&
mAreaEnu
=
this
->
pImpl
->
scenario
.
measurementArea
();
auto
&
mArea
=
this
->
pImpl
->
mArea
;
mAreaEnu
.
clear
();
for
(
auto
geoVertex
:
mArea
){
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
mAreaEnu
.
outer
().
push_back
(
p
);
}
// Service area update necessary.
if
(
_sArea
.
size
()
!=
_pScenario
->
serviceArea
().
outer
().
size
()
){
for
(
auto
geoVertex
:
_sArea
){
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
_pScenario
->
serviceArea
().
outer
().
push_back
(
p
);
}
// Update service area.
auto
&
sAreaEnu
=
this
->
pImpl
->
scenario
.
serviceArea
();
auto
&
sArea
=
this
->
pImpl
->
sArea
;
sAreaEnu
.
clear
();
for
(
auto
geoVertex
:
sArea
){
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
sAreaEnu
.
outer
().
push_back
(
p
);
}
// Service area update necessary.
if
(
_corridor
.
size
()
!=
_pScenario
->
corridor
().
outer
().
size
()
){
for
(
auto
geoVertex
:
_corridor
){
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
_pScenario
->
corridor
().
outer
().
push_back
(
p
);
}
// Update corridor.
auto
&
corridorEnu
=
this
->
pImpl
->
scenario
.
corridor
();
auto
&
corridor
=
this
->
pImpl
->
corridor
;
corridor
.
clear
();
for
(
auto
geoVertex
:
corridor
){
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
corridorEnu
.
outer
().
push_back
(
p
);
}
if
(
!
_pScenario
->
update
()
){
{
QMutexLocker
lk
(
&
_pImpl
->
m
);
for
(
auto
c
:
_pScenario
->
errorString
){
_pImpl
->
errorMessage
.
push_back
(
QChar
(
c
));
}
}
if
(
!
this
->
pImpl
->
scenario
.
update
()
){
pImpl
->
errorMessage
=
this
->
pImpl
->
scenario
.
errorString
.
c_str
();
return
;
}
// Asynchronously update flightplan.
auto
future
=
std
::
async
(
std
::
bind
(
&
snake
::
Flightplan
::
update
,
_pFlightplan
));
qWarning
()
<<
"route calculation missing"
;
std
::
string
errorString
;
auto
future
=
std
::
async
([
this
,
&
errorString
]{
auto
alpha
=
-
this
->
pImpl
->
scenario
.
mAreaBoundingBox
().
angle
*
degree
::
degree
;
snake
::
flight_plan
::
Transects
transects
;
transects
.
push_back
(
bg
::
model
::
linestring
{
this
->
pImpl
->
scenario
.
homePositon
()});
bool
value
=
snake
::
flight_plan
::
transectsFromScenario
(
this
->
pImpl
->
lineDistance
,
this
->
pImpl
->
minTransectLength
,
alpha
,
this
->
pImpl
->
scenario
,
this
->
pImpl
->
progress
,
transects
,
errorString
);
if
(
!
value
)
return
value
;
snake
::
flight_plan
::
Transects
transectsRouted
;
snake
::
flight_plan
::
Route
route
;
value
=
snake
::
flight_plan
::
route
(
this
->
pImpl
->
scenario
.
joinedArea
(),
transects
,
transectsRouted
,
route
,
errorString
);
if
(
!
value
)
return
value
;
// Store waypoints.
for
(
auto
boostVertex
:
_pFlightplan
->
waypoints
()){
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
pImpl
->
waypointsENU
.
push_back
(
enuVertex
);
pImpl
->
waypoints
.
push_back
(
geoVertex
);
}
// Store arrival path.
for
(
auto
boostVertex
:
_pFlightplan
->
arrivalPath
()){
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
pImpl
->
arrivalPathENU
.
push_back
(
enuVertex
);
pImpl
->
arrivalPath
.
push_back
(
geoVertex
);
}
// Store return path.
for
(
auto
boostVertex
:
_pFlightplan
->
returnPath
()){
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
pImpl
->
returnPathENU
.
push_back
(
enuVertex
);
pImpl
->
returnPath
.
push_back
(
geoVertex
);
}
return
true
;
});
// Continue with storing scenario data in the mean time.
{
QMutexLocker
lk
(
&
_pImpl
->
m
);
// Get tiles.
const
auto
&
tiles
=
_pScenario
->
tiles
();
const
auto
&
centerPoints
=
_pScenario
->
tileCenterPoints
();
const
auto
&
tiles
=
this
->
pImpl
->
scenario
.
tiles
();
const
auto
&
centerPoints
=
this
->
pImpl
->
scenario
.
tileCenterPoints
();
for
(
unsigned
int
i
=
0
;
i
<
tiles
.
size
();
++
i
)
{
const
auto
&
tile
=
tiles
[
i
];
SnakeTile
geoTile
;
...
...
@@ -296,10 +359,10 @@ void SnakeDataManager::run()
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostPoint
,
geoVertex
);
geoTile
.
setCenter
(
geoVertex
);
_
pImpl
->
tiles
.
polygons
().
push_back
(
geoTile
);
_
pImpl
->
tileCenterPoints
.
push_back
(
QVariant
::
fromValue
(
geoVertex
));
_
pImpl
->
tilesENU
.
polygons
().
push_back
(
enuTile
);
_
pImpl
->
tileCenterPointsENU
.
push_back
(
enuVertex
);
pImpl
->
tiles
.
polygons
().
push_back
(
geoTile
);
pImpl
->
tileCenterPoints
.
push_back
(
QVariant
::
fromValue
(
geoVertex
));
pImpl
->
tilesENU
.
polygons
().
push_back
(
enuTile
);
pImpl
->
tileCenterPointsENU
.
push_back
(
enuVertex
);
}
}
...
...
@@ -307,37 +370,12 @@ void SnakeDataManager::run()
// Trying to generate flight plan.
if
(
!
future
.
get
()
){
// error
QMutexLocker
lk
(
&
_pImpl
->
m
);
for
(
auto
c
:
_pFlightplan
->
errorString
){
_
pImpl
->
errorMessage
.
push_back
(
QChar
(
c
));
pImpl
->
errorMessage
.
push_back
(
QChar
(
c
));
}
}
else
{
//success!!!
QMutexLocker
lk
(
&
_pImpl
->
m
);
// Store waypoints.
for
(
auto
boostVertex
:
_pFlightplan
->
waypoints
()){
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
_pImpl
->
waypointsENU
.
push_back
(
enuVertex
);
_pImpl
->
waypoints
.
push_back
(
geoVertex
);
}
// Store arrival path.
for
(
auto
boostVertex
:
_pFlightplan
->
arrivalPath
()){
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
_pImpl
->
arrivalPathENU
.
push_back
(
enuVertex
);
_pImpl
->
arrivalPath
.
push_back
(
geoVertex
);
}
// Store return path.
for
(
auto
boostVertex
:
_pFlightplan
->
returnPath
()){
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
_pImpl
->
returnPathENU
.
push_back
(
enuVertex
);
_pImpl
->
returnPath
.
push_back
(
geoVertex
);
}
// Success!!!
}
}
...
...
src/Wima/Snake/SnakeDataManager.h
View file @
f3a5cb44
...
...
@@ -57,7 +57,8 @@ protected:
private:
bool
precondition
()
const
;
SnakeImplPtr
_pImpl
;
void
resetWaypointData
();
SnakeImplPtr
pImpl
;
};
...
...
src/Wima/WimaController.cc
View file @
f3a5cb44
...
...
@@ -200,6 +200,18 @@ Fact *WimaController::altitude() {
return
&
_altitude
;
}
QmlObjectListModel
*
WimaController
::
snakeTiles
()
{
qWarning
()
<<
"using snake tile dummy"
;
return
QmlObjectListModel
();
}
QVariantList
WimaController
::
snakeTileCenterPoints
()
{
qWarning
()
<<
"using snakeTileCenterPoints dummy"
;
return
QVariantList
();
}
QVector
<
int
>
WimaController
::
nemoProgress
()
{
if
(
_nemoProgress
.
progress
().
size
()
==
_snakeTileCenterPoints
.
size
()
)
return
_nemoProgress
.
progress
();
...
...
src/Wima/WimaController.h
View file @
f3a5cb44
...
...
@@ -216,8 +216,8 @@ public:
Fact
*
snakeLineDistance
(
void
)
{
return
&
_snakeLineDistance
;}
Fact
*
snakeMinTransectLength
(
void
)
{
return
&
_snakeMinTransectLength
;}
// Snake data.
QmlObjectListModel
*
snakeTiles
(
void
)
{
return
_snakeTiles
.
QmlObjectListModel
();}
QVariantList
snakeTileCenterPoints
(
void
)
{
return
_snakeTileCenterPoints
;}
QmlObjectListModel
*
snakeTiles
(
void
)
;
QVariantList
snakeTileCenterPoints
(
void
)
;
QVector
<
int
>
nemoProgress
(
void
);
int
nemoStatus
(
void
)
const
;
QString
nemoStatusString
(
void
)
const
;
...
...
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