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
625b439c
Commit
625b439c
authored
Sep 19, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
123
parent
7271b853
Changes
7
Expand all
Show whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
830 additions
and
583 deletions
+830
-583
CS_old.cc
src/Wima/CS_old.cc
+737
-0
CircularSurvey.cc
src/Wima/CircularSurvey.cc
+55
-33
SnakeThread.cc
src/Wima/Snake/SnakeThread.cc
+11
-5
route.cpp
src/Wima/Snake/route.cpp
+0
-183
snake.cpp
src/Wima/Snake/snake.cpp
+13
-11
snake.h
src/Wima/Snake/snake.h
+14
-3
snake_old.cpp
src/Wima/Snake/snake_old.cpp
+0
-348
No files found.
src/Wima/CS_old.cc
0 → 100644
View file @
625b439c
This diff is collapsed.
Click to expand it.
src/Wima/CircularSurvey.cc
View file @
625b439c
...
@@ -387,11 +387,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -387,11 +387,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
snake
::
BoostPolygon
polygonENU
;
snake
::
BoostPolygon
polygonENU
;
snake
::
BoostPoint
originENU
{
0
,
0
};
snake
::
BoostPoint
originENU
{
0
,
0
};
snake
::
areaToEnu
(
origin
,
polygon
,
polygonENU
);
snake
::
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
string
error
;
// Check validity.
// Check validity.
if
(
!
bg
::
is_valid
(
polygonENU
))
{
if
(
!
bg
::
is_valid
(
polygonENU
,
error
))
{
#ifdef DEBUG_CIRCULAR_SURVEY
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
"invalid polygon."
;
"invalid polygon."
;
qWarning
()
<<
error
.
c_str
();
#endif
#endif
return
PtrTransects
();
return
PtrTransects
();
}
else
{
}
else
{
...
@@ -401,21 +403,27 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -401,21 +403,27 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
std
::
vector
<
snake
::
Angle
>
angles
;
std
::
vector
<
snake
::
Angle
>
angles
;
angles
.
reserve
(
polygonENU
.
outer
().
size
());
angles
.
reserve
(
polygonENU
.
outer
().
size
());
for
(
const
auto
&
p
:
polygonENU
.
outer
())
{
for
(
const
auto
&
p
:
polygonENU
.
outer
())
{
distances
.
push_back
(
bg
::
distance
(
originENU
,
p
)
*
si
::
meter
);
snake
::
Length
distance
=
bg
::
distance
(
originENU
,
p
)
*
si
::
meter
;
angles
.
push_back
((
std
::
atan2
(
p
.
get
<
1
>
(),
p
.
get
<
0
>
())
+
M_PI
)
*
distances
.
push_back
(
distance
);
si
::
radian
);
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
auto
rMin
=
deltaR
;
// minimal circle radius
snake
::
Angle
alpha1
(
0
*
degree
::
degree
);
snake
::
Angle
alpha1
(
0
*
degree
::
degree
);
snake
::
Angle
alpha2
(
360
*
degree
::
degree
);
snake
::
Angle
alpha2
(
360
*
degree
::
degree
);
bool
refInside
=
true
;
// Determine r_min by successive approximation
// Determine r_min by successive approximation
if
(
!
bg
::
within
(
originENU
,
polygonENU
))
{
if
(
!
bg
::
within
(
originENU
,
polygonENU
))
{
rMin
=
bg
::
distance
(
originENU
,
polygonENU
)
*
si
::
meter
;
rMin
=
bg
::
distance
(
originENU
,
polygonENU
)
*
si
::
meter
;
alpha1
=
(
*
std
::
min_element
(
angles
.
begin
(),
angles
.
end
()));
alpha2
=
(
*
std
::
max_element
(
angles
.
begin
(),
angles
.
end
()));
refInside
=
false
;
}
}
auto
rMax
=
auto
rMax
=
...
@@ -431,44 +439,31 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -431,44 +439,31 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
0
>
())),
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
0
>
())),
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
1
>
()))};
ClipperLib
::
cInt
(
std
::
round
(
originENU
.
get
<
1
>
()))};
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto
s1
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
// Generate circle sectors.
// Generate circle sectors.
auto
rScaled
=
rMinScaled
;
auto
rScaled
=
rMinScaled
;
const
auto
nTran
=
long
(
std
::
floor
(((
rMax
-
rMin
)
/
deltaR
).
value
()));
const
auto
nTran
=
long
(
std
::
ceil
(((
rMax
-
rMin
)
/
deltaR
).
value
()));
vector
<
ClipperLib
::
Path
>
sectors
(
nTran
,
ClipperLib
::
Path
());
vector
<
ClipperLib
::
Path
>
sectors
(
nTran
,
ClipperLib
::
Path
());
const
auto
nSectors
=
const
auto
nSectors
=
long
(
std
::
round
(((
alpha2
-
alpha1
)
/
deltaAlpha
).
value
()));
long
(
std
::
round
(((
alpha2
-
alpha1
)
/
deltaAlpha
).
value
()));
#ifdef DEBUG_CIRCULAR_SURVEY
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"sector parameres:"
;
qWarning
()
<<
"sector parameres:"
;
qWarning
()
<<
"alpha1: "
<<
to_string
(
alpha1
).
c_str
();
qWarning
()
<<
"alpha1: "
<<
to_string
(
snake
::
Degree
(
alpha1
)
).
c_str
();
qWarning
()
<<
"alpha2: "
<<
to_string
(
alpha2
).
c_str
();
qWarning
()
<<
"alpha2: "
<<
to_string
(
snake
::
Degree
(
alpha2
)
).
c_str
();
qWarning
()
<<
"n: "
qWarning
()
<<
"n: "
<<
to_string
((
alpha2
-
alpha1
)
/
deltaAlpha
).
c_str
();
<<
to_string
((
alpha2
-
alpha1
)
/
deltaAlpha
).
c_str
();
qWarning
()
<<
"nSectors: "
<<
nSectors
;
qWarning
()
<<
"nSectors: "
<<
nSectors
;
qWarning
()
<<
"rMin: "
<<
to_string
(
rMin
).
c_str
();
qWarning
()
<<
"rMin: "
<<
to_string
(
rMin
).
c_str
();
qWarning
()
<<
"rMax: "
<<
to_string
(
rMax
).
c_str
();
qWarning
()
<<
"rMax: "
<<
to_string
(
rMax
).
c_str
();
qWarning
()
<<
"nTran: "
<<
nTran
;
qWarning
()
<<
"nTran: "
<<
nTran
;
qWarning
()
<<
"refInside: "
<<
refInside
;
#endif
#endif
using
ClipperCircle
=
using
ClipperCircle
=
GenericCircle
<
ClipperLib
::
cInt
,
ClipperLib
::
IntPoint
>
;
GenericCircle
<
ClipperLib
::
cInt
,
ClipperLib
::
IntPoint
>
;
// Helper lambda.
std
::
function
<
void
(
ClipperCircle
&
circle
,
ClipperLib
::
Path
&
sectors
)
>
approx
;
if
(
refInside
)
{
approx
=
[
nSectors
](
ClipperCircle
&
circle
,
ClipperLib
::
Path
&
sector
)
{
approximate
(
circle
,
nSectors
,
sector
);
};
}
else
{
approx
=
[
nSectors
,
alpha1
,
alpha2
](
ClipperCircle
&
circle
,
ClipperLib
::
Path
&
sector
)
{
approximate
(
circle
,
nSectors
,
alpha1
,
alpha2
,
sector
);
};
}
for
(
auto
&
sector
:
sectors
)
{
for
(
auto
&
sector
:
sectors
)
{
ClipperCircle
circle
(
rScaled
,
originScaled
);
ClipperCircle
circle
(
rScaled
,
originScaled
);
approx
(
circle
,
sector
);
approx
imate
(
circle
,
nSectors
,
sector
);
rScaled
+=
deltaRScaled
;
rScaled
+=
deltaRScaled
;
}
}
// Clip sectors to polygonENU.
// Clip sectors to polygonENU.
...
@@ -502,6 +497,21 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -502,6 +497,21 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
transectsENU
.
push_back
(
transect
);
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
)
{
if
(
transectsENU
.
size
()
==
0
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
#ifdef DEBUG_CIRCULAR_SURVEY
...
@@ -512,10 +522,10 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -512,10 +522,10 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
}
}
// Route transects;
// Route transects;
s
nake
::
Transects
transectsRouted
;
s
td
::
vector
<
snake
::
TransectInfo
>
transectsInfo
;
snake
::
Route
route
;
snake
::
Route
route
;
std
::
string
errorString
;
std
::
string
errorString
;
bool
success
=
snake
::
route
(
polygonENU
,
transectsENU
,
transects
Routed
,
bool
success
=
snake
::
route
(
polygonENU
,
transectsENU
,
transects
Info
,
route
,
errorString
);
route
,
errorString
);
if
(
!
success
)
{
if
(
!
success
)
{
#ifdef DEBUG_CIRCULAR_SURVEY
#ifdef DEBUG_CIRCULAR_SURVEY
...
@@ -525,6 +535,18 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -525,6 +535,18 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
return
PtrTransects
();
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
;
QList
<
CoordInfo_t
>
transectList
;
transectList
.
reserve
(
route
.
size
());
transectList
.
reserve
(
route
.
size
());
for
(
const
auto
&
vertex
:
route
)
{
for
(
const
auto
&
vertex
:
route
)
{
...
@@ -533,13 +555,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -533,13 +555,13 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
CoordInfo_t
coordinfo
=
{
c
,
CoordTypeInterior
};
CoordInfo_t
coordinfo
=
{
c
,
CoordTypeInterior
};
transectList
.
append
(
coordinfo
);
transectList
.
append
(
coordinfo
);
}
}
PtrTransects
t
ransects
(
new
Transects
());
PtrTransects
pT
ransects
(
new
Transects
());
t
ransects
->
append
(
transectList
);
pT
ransects
->
append
(
transectList
);
#ifdef DEBUG_CIRCULAR_SURVEY
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
qWarning
()
<<
"CircularSurvey::_rebuildTransectsPhase1(): "
"concurrent update success."
;
"concurrent update success."
;
#endif
#endif
return
t
ransects
;
return
pT
ransects
;
}
}
});
// QtConcurrent::run()
});
// QtConcurrent::run()
_watcher
.
setFuture
(
future
);
_watcher
.
setFuture
(
future
);
...
...
src/Wima/Snake/SnakeThread.cc
View file @
625b439c
...
@@ -360,7 +360,7 @@ void SnakeThread::run() {
...
@@ -360,7 +360,7 @@ void SnakeThread::run() {
bool
waypointsValid
=
true
;
bool
waypointsValid
=
true
;
snake
::
Transects
transects
;
snake
::
Transects
transects
;
s
nake
::
Transects
transectsRouted
;
s
td
::
vector
<
snake
::
TransectInfo
>
transectsInfo
;
snake
::
Route
route
;
snake
::
Route
route
;
bool
needWaypointUpdate
=
this
->
pImpl
->
input
.
scenarioChanged
||
bool
needWaypointUpdate
=
this
->
pImpl
->
input
.
scenarioChanged
||
this
->
pImpl
->
input
.
routeParametersChanged
||
this
->
pImpl
->
input
.
routeParametersChanged
||
...
@@ -385,8 +385,8 @@ void SnakeThread::run() {
...
@@ -385,8 +385,8 @@ void SnakeThread::run() {
waypointsValid
=
false
;
waypointsValid
=
false
;
}
else
if
(
transects
.
size
()
>
1
)
{
}
else
if
(
transects
.
size
()
>
1
)
{
// Route transects.
// Route transects.
success
=
snake
::
route
(
this
->
pImpl
->
jAreaENU
,
transects
,
success
=
snake
::
route
(
this
->
pImpl
->
jAreaENU
,
transects
,
transectsInfo
,
transectsRouted
,
route
,
errorString
);
route
,
errorString
);
if
(
!
success
)
{
if
(
!
success
)
{
UniqueLock
lk
(
this
->
pImpl
->
output
.
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
output
.
mutex
);
this
->
pImpl
->
output
.
errorMessage
=
errorString
.
c_str
();
this
->
pImpl
->
output
.
errorMessage
=
errorString
.
c_str
();
...
@@ -481,7 +481,10 @@ void SnakeThread::run() {
...
@@ -481,7 +481,10 @@ void SnakeThread::run() {
this
->
pImpl
->
output
.
waypoints
.
clear
();
this
->
pImpl
->
output
.
waypoints
.
clear
();
this
->
pImpl
->
output
.
waypointsENU
.
clear
();
this
->
pImpl
->
output
.
waypointsENU
.
clear
();
// Store arrival path.
// Store arrival path.
const
auto
&
firstWaypoint
=
transectsRouted
.
front
().
front
();
const
auto
&
info
=
transectsInfo
.
front
();
const
auto
&
firstTransect
=
transects
[
info
.
index
];
const
auto
&
firstWaypoint
=
info
.
reversed
?
firstTransect
.
front
()
:
firstTransect
.
back
();
long
startIdx
=
0
;
long
startIdx
=
0
;
for
(
long
i
=
0
;
i
<
long
(
route
.
size
());
++
i
)
{
for
(
long
i
=
0
;
i
<
long
(
route
.
size
());
++
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
const
auto
&
boostVertex
=
route
[
i
];
...
@@ -497,7 +500,10 @@ void SnakeThread::run() {
...
@@ -497,7 +500,10 @@ void SnakeThread::run() {
}
}
// Store return path.
// Store return path.
long
endIdx
=
0
;
long
endIdx
=
0
;
const
auto
&
lastWaypoint
=
transectsRouted
.
back
().
back
();
const
auto
&
info2
=
transectsInfo
.
back
();
const
auto
&
lastTransect
=
transects
[
info2
.
index
];
const
auto
&
lastWaypoint
=
info2
.
reversed
?
lastTransect
.
front
()
:
lastTransect
.
back
();
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
lastWaypoint
)
{
if
(
boostVertex
==
lastWaypoint
)
{
...
...
src/Wima/Snake/route.cpp
deleted
100644 → 0
View file @
7271b853
bool
flight_plan
::
route
(
const
BoostPolygon
&
area
,
const
flight_plan
::
Transects
&
transects
,
Transects
&
transectsRouted
,
flight_plan
::
Route
&
route
,
string
&
errorString
)
{
//=======================================
// Route Transects using Google or-tools.
//=======================================
// Create vertex list;
BoostLineString
vertices
;
size_t
n0
=
0
;
for
(
const
auto
&
t
:
transects
)
{
n0
+=
std
::
min
<
std
::
size_t
>
(
t
.
size
(),
2
);
}
vertices
.
reserve
(
n0
);
struct
TransectInfo
{
TransectInfo
(
size_t
n
,
bool
f
)
:
index
(
n
),
front
(
f
)
{}
size_t
index
;
bool
front
;
};
std
::
vector
<
TransectInfo
>
transectInfoList
;
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
)
{
const
auto
&
t
=
transects
[
i
];
vertices
.
push_back
(
t
.
front
());
transectInfoList
.
push_back
(
TransectInfo
{
i
,
true
});
if
(
t
.
size
()
>=
2
)
{
vertices
.
push_back
(
t
.
back
());
transectInfoList
.
push_back
(
TransectInfo
{
i
,
false
});
}
}
for
(
long
i
=
0
;
i
<
long
(
area
.
outer
().
size
())
-
1
;
++
i
)
{
vertices
.
push_back
(
area
.
outer
()[
i
]);
}
for
(
auto
&
ring
:
area
.
inners
())
{
for
(
auto
&
vertex
:
ring
)
vertices
.
push_back
(
vertex
);
}
size_t
n1
=
vertices
.
size
();
// Generate routing model.
RoutingDataModel
dataModel
;
Matrix
<
double
>
connectionGraph
(
n1
,
n1
);
// Offset joined area.
BoostPolygon
areaOffset
;
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
#ifdef SNAKE_SHOW_TIME
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
generateRoutingModel
(
vertices
,
areaOffset
,
n0
,
dataModel
,
connectionGraph
);
#ifdef SNAKE_SHOW_TIME
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
// Create Routing Index Manager.
RoutingIndexManager
manager
(
dataModel
.
distanceMatrix
.
getN
(),
dataModel
.
numVehicles
,
dataModel
.
depot
);
// Create Routing Model.
RoutingModel
routing
(
manager
);
// Create and register a transit callback.
const
int
transitCallbackIndex
=
routing
.
RegisterTransitCallback
(
[
&
dataModel
,
&
manager
](
int64
from_index
,
int64
to_index
)
->
int64
{
// Convert from routing variable Index to distance matrix NodeIndex.
auto
from_node
=
manager
.
IndexToNode
(
from_index
).
value
();
auto
to_node
=
manager
.
IndexToNode
(
to_index
).
value
();
return
dataModel
.
distanceMatrix
.
get
(
from_node
,
to_node
);
});
// Define cost of each arc.
routing
.
SetArcCostEvaluatorOfAllVehicles
(
transitCallbackIndex
);
// Define Constraints (this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
#ifdef SNAKE_DEBUG
std
::
cout
<<
"snake::route(): Constraints:"
<<
std
::
endl
;
#endif
Solver
*
solver
=
routing
.
solver
();
size_t
index
=
0
;
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
)
{
const
auto
&
t
=
transects
[
i
];
#ifdef SNAKE_DEBUG
std
::
cout
<<
"i = "
<<
i
<<
": t.size() = "
<<
t
.
size
()
<<
std
::
endl
;
#endif
if
(
t
.
size
()
>=
2
)
{
auto
idx0
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
index
));
auto
idx1
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
index
+
1
));
auto
cond0
=
routing
.
NextVar
(
idx0
)
->
IsEqual
(
idx1
);
auto
cond1
=
routing
.
NextVar
(
idx1
)
->
IsEqual
(
idx0
);
auto
c
=
solver
->
MakeNonEquality
(
cond0
,
cond1
);
solver
->
AddConstraint
(
c
);
#ifdef SNAKE_DEBUG
std
::
cout
<<
"( next("
<<
index
<<
") == "
<<
index
+
1
<<
" ) != ( next("
<<
index
+
1
<<
") == "
<<
index
<<
" )"
<<
std
::
endl
;
#endif
index
+=
2
;
}
else
{
index
+=
1
;
}
}
// Setting first solution heuristic.
RoutingSearchParameters
searchParameters
=
DefaultRoutingSearchParameters
();
searchParameters
.
set_first_solution_strategy
(
FirstSolutionStrategy
::
PATH_CHEAPEST_ARC
);
google
::
protobuf
::
Duration
*
tMax
=
new
google
::
protobuf
::
Duration
();
// seconds
tMax
->
set_seconds
(
10
);
searchParameters
.
set_allocated_time_limit
(
tMax
);
// Solve the problem.
#ifdef SNAKE_SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
const
Assignment
*
solution
=
routing
.
SolveWithParameters
(
searchParameters
);
#ifdef SNAKE_SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time routing.SolveWithParameters(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
if
(
!
solution
||
solution
->
Size
()
<=
1
)
{
errorString
=
"Not able to solve the routing problem."
;
return
false
;
}
// Extract index list from solution.
index
=
routing
.
Start
(
0
);
std
::
vector
<
size_t
>
route_idx
;
route_idx
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
while
(
!
routing
.
IsEnd
(
index
))
{
index
=
solution
->
Value
(
routing
.
NextVar
(
index
));
route_idx
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
}
// Helper Lambda.
auto
idx2Vertex
=
[
&
vertices
](
const
std
::
vector
<
size_t
>
&
idxArray
,
std
::
vector
<
BoostPoint
>
&
path
)
{
for
(
auto
idx
:
idxArray
)
path
.
push_back
(
vertices
[
idx
]);
};
// Construct route.
for
(
size_t
i
=
0
;
i
<
route_idx
.
size
()
-
1
;
++
i
)
{
size_t
idx0
=
route_idx
[
i
];
size_t
idx1
=
route_idx
[
i
+
1
];
const
auto
&
info1
=
transectInfoList
[
idx0
];
const
auto
&
info2
=
transectInfoList
[
idx1
];
if
(
info1
.
index
==
info2
.
index
)
{
// same transect?
if
(
!
info1
.
front
)
{
// transect reversal needed?
BoostLineString
reversedTransect
;
const
auto
&
t
=
transects
[
info1
.
index
];
for
(
auto
it
=
t
.
end
()
-
1
;
it
>=
t
.
begin
();
--
it
)
{
reversedTransect
.
push_back
(
*
it
);
}
transectsRouted
.
push_back
(
reversedTransect
);
for
(
auto
it
=
reversedTransect
.
begin
();
it
<
reversedTransect
.
end
()
-
1
;
++
it
)
{
route
.
push_back
(
*
it
);
}
}
else
{
const
auto
&
t
=
transects
[
info1
.
index
];
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
)
{
route
.
push_back
(
*
it
);
}
transectsRouted
.
push_back
(
t
);
}
}
else
{
std
::
vector
<
size_t
>
idxList
;
shortestPathFromGraph
(
connectionGraph
,
idx0
,
idx1
,
idxList
);
if
(
i
!=
route_idx
.
size
()
-
2
)
{
idxList
.
pop_back
();
}
idx2Vertex
(
idxList
,
route
);
}
}
return
true
;
}
src/Wima/Snake/snake.cpp
View file @
625b439c
...
@@ -781,7 +781,8 @@ void generateRoutingModel(const BoostLineString &vertices,
...
@@ -781,7 +781,8 @@ void generateRoutingModel(const BoostLineString &vertices,
}
}
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
Transects
&
transectsRouted
,
Route
&
route
,
string
&
errorString
)
{
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
route
,
string
&
errorString
)
{
//=======================================
//=======================================
// Route Transects using Google or-tools.
// Route Transects using Google or-tools.
//=======================================
//=======================================
...
@@ -794,19 +795,19 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -794,19 +795,19 @@ bool route(const BoostPolygon &area, const Transects &transects,
}
}
vertices
.
reserve
(
n0
);
vertices
.
reserve
(
n0
);
struct
Transect
Info
{
struct
Local
Info
{
Transect
Info
(
size_t
n
,
bool
f
)
:
index
(
n
),
front
(
f
)
{}
Local
Info
(
size_t
n
,
bool
f
)
:
index
(
n
),
front
(
f
)
{}
size_t
index
;
size_t
index
;
bool
front
;
bool
front
;
};
};
std
::
vector
<
TransectInfo
>
transectInfoList
;
std
::
vector
<
LocalInfo
>
localTransectInfo
;
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
)
{
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
)
{
const
auto
&
t
=
transects
[
i
];
const
auto
&
t
=
transects
[
i
];
vertices
.
push_back
(
t
.
front
());
vertices
.
push_back
(
t
.
front
());
transectInfoList
.
push_back
(
Transect
Info
{
i
,
true
});
localTransectInfo
.
push_back
(
Local
Info
{
i
,
true
});
if
(
t
.
size
()
>=
2
)
{
if
(
t
.
size
()
>=
2
)
{
vertices
.
push_back
(
t
.
back
());
vertices
.
push_back
(
t
.
back
());
transectInfoList
.
push_back
(
Transect
Info
{
i
,
false
});
localTransectInfo
.
push_back
(
Local
Info
{
i
,
false
});
}
}
}
}
...
@@ -929,16 +930,18 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -929,16 +930,18 @@ bool route(const BoostPolygon &area, const Transects &transects,
for
(
size_t
i
=
0
;
i
<
route_idx
.
size
()
-
1
;
++
i
)
{
for
(
size_t
i
=
0
;
i
<
route_idx
.
size
()
-
1
;
++
i
)
{
size_t
idx0
=
route_idx
[
i
];
size_t
idx0
=
route_idx
[
i
];
size_t
idx1
=
route_idx
[
i
+
1
];
size_t
idx1
=
route_idx
[
i
+
1
];
const
auto
&
info1
=
transectInfoList
[
idx0
];
const
auto
&
info1
=
localTransectInfo
[
idx0
];
const
auto
&
info2
=
transectInfoList
[
idx1
];
const
auto
&
info2
=
localTransectInfo
[
idx1
];
if
(
info1
.
index
==
info2
.
index
)
{
// same transect?
if
(
info1
.
index
==
info2
.
index
)
{
// same transect?
TransectInfo
trInfo
(
info1
.
index
,
!
info1
.
front
?
true
:
false
/*transect reversed?*/
);
transectInfo
.
push_back
(
trInfo
);
if
(
!
info1
.
front
)
{
// transect reversal needed?
if
(
!
info1
.
front
)
{
// transect reversal needed?
BoostLineString
reversedTransect
;
BoostLineString
reversedTransect
;
const
auto
&
t
=
transects
[
info1
.
index
];
const
auto
&
t
=
transects
[
info1
.
index
];
for
(
auto
it
=
t
.
end
()
-
1
;
it
>=
t
.
begin
();
--
it
)
{
for
(
auto
it
=
t
.
end
()
-
1
;
it
>=
t
.
begin
();
--
it
)
{
reversedTransect
.
push_back
(
*
it
);
reversedTransect
.
push_back
(
*
it
);
}
}
transectsRouted
.
push_back
(
reversedTransect
);
for
(
auto
it
=
reversedTransect
.
begin
();
for
(
auto
it
=
reversedTransect
.
begin
();
it
<
reversedTransect
.
end
()
-
1
;
++
it
)
{
it
<
reversedTransect
.
end
()
-
1
;
++
it
)
{
route
.
push_back
(
*
it
);
route
.
push_back
(
*
it
);
...
@@ -948,7 +951,6 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -948,7 +951,6 @@ bool route(const BoostPolygon &area, const Transects &transects,
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
)
{
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
)
{
route
.
push_back
(
*
it
);
route
.
push_back
(
*
it
);
}
}
transectsRouted
.
push_back
(
t
);
}
}
}
else
{
}
else
{
std
::
vector
<
size_t
>
idxList
;
std
::
vector
<
size_t
>
idxList
;
...
...
src/Wima/Snake/snake.h
View file @
625b439c
...
@@ -98,7 +98,9 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
...
@@ -98,7 +98,9 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
origin
.
altitude
(),
earth
);
origin
.
altitude
(),
earth
);
double
x
=
0
,
y
=
0
,
z
=
0
;
double
x
=
0
,
y
=
0
,
z
=
0
;
proj
.
Forward
(
in
.
latitude
(),
in
.
longitude
(),
in
.
altitude
(),
x
,
y
,
z
);
auto
alt
=
in
.
altitude
();
alt
=
std
::
isnan
(
alt
)
?
0
:
alt
;
proj
.
Forward
(
in
.
latitude
(),
in
.
longitude
(),
alt
,
x
,
y
,
z
);
out
.
set
<
0
>
(
x
);
out
.
set
<
0
>
(
x
);
out
.
set
<
1
>
(
y
);
out
.
set
<
1
>
(
y
);
(
void
)
z
;
(
void
)
z
;
...
@@ -156,6 +158,8 @@ void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
...
@@ -156,6 +158,8 @@ void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
typedef
bu
::
quantity
<
bu
::
si
::
length
>
Length
;
typedef
bu
::
quantity
<
bu
::
si
::
length
>
Length
;
typedef
bu
::
quantity
<
bu
::
si
::
area
>
Area
;
typedef
bu
::
quantity
<
bu
::
si
::
area
>
Area
;
typedef
bu
::
quantity
<
bu
::
si
::
plane_angle
>
Angle
;
typedef
bu
::
quantity
<
bu
::
si
::
plane_angle
>
Angle
;
typedef
bu
::
quantity
<
bu
::
si
::
plane_angle
>
Radian
;
typedef
bu
::
quantity
<
bu
::
degree
::
plane_angle
>
Degree
;
bool
joinedArea
(
const
std
::
vector
<
BoostPolygon
*>
&
areas
,
BoostPolygon
&
jArea
);
bool
joinedArea
(
const
std
::
vector
<
BoostPolygon
*>
&
areas
,
BoostPolygon
&
jArea
);
bool
joinedArea
(
const
BoostPolygon
&
mArea
,
const
BoostPolygon
&
sArea
,
bool
joinedArea
(
const
BoostPolygon
&
mArea
,
const
BoostPolygon
&
sArea
,
...
@@ -174,12 +178,19 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle,
...
@@ -174,12 +178,19 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const
std
::
vector
<
BoostPolygon
>
&
tiles
,
const
std
::
vector
<
BoostPolygon
>
&
tiles
,
const
Progress
&
p
,
Transects
&
t
,
const
Progress
&
p
,
Transects
&
t
,
string
&
errorString
);
string
&
errorString
);
struct
TransectInfo
{
TransectInfo
(
size_t
n
,
bool
r
)
:
index
(
n
),
reversed
(
r
)
{}
size_t
index
;
bool
reversed
;
};
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
Transects
&
transectsRouted
,
Route
&
route
,
string
&
errorString
);
std
::
vector
<
TransectInfo
>
&
transectInfo
,
Route
&
route
,
string
&
errorString
);
namespace
detail
{
namespace
detail
{
const
double
offsetConstant
=
const
double
offsetConstant
=
1
;
// meter, polygon offset to compenstate for numerical inaccurracies.
0
.
1
;
// meter, polygon offset to compenstate for numerical inaccurracies.
}
// namespace detail
}
// namespace detail
}
// namespace snake
}
// namespace snake
...
...
src/Wima/Snake/snake_old.cpp
deleted
100644 → 0
View file @
7271b853
This diff is collapsed.
Click to expand it.
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