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
990aa5eb
Commit
990aa5eb
authored
Sep 09, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
temp
parent
c6e6a519
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
140 additions
and
106 deletions
+140
-106
snake.cpp
src/Snake/snake.cpp
+42
-23
snake.h
src/Snake/snake.h
+5
-5
SnakeDataManager.cc
src/Wima/Snake/SnakeDataManager.cc
+88
-76
WimaController.cc
src/Wima/WimaController.cc
+5
-2
No files found.
src/Snake/snake.cpp
View file @
990aa5eb
#include <algorithm>
#include <iostream>
#include <iostream>
#include "snake.h"
#include "snake.h"
...
@@ -52,7 +53,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) {
...
@@ -52,7 +53,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) {
center
.
set
<
1
>
(
c
.
y
);
center
.
set
<
1
>
(
c
.
y
);
}
}
void
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
BoundingBox
&
minBBox
)
{
bool
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
BoundingBox
&
minBBox
)
{
/*
/*
Find the minimum-area bounding box of a set of 2D points
Find the minimum-area bounding box of a set of 2D points
...
@@ -94,8 +95,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
...
@@ -94,8 +95,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
POSSIBILITY OF SUCH DAMAGE.
POSSIBILITY OF SUCH DAMAGE.
*/
*/
if
(
polygon
.
outer
().
empty
())
if
(
polygon
.
outer
().
empty
()
||
polygon
.
outer
().
size
()
<
3
)
return
;
return
false
;
BoostPolygon
convex_hull
;
BoostPolygon
convex_hull
;
bg
::
convex_hull
(
polygon
,
convex_hull
);
bg
::
convex_hull
(
polygon
,
convex_hull
);
...
@@ -198,6 +199,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
...
@@ -198,6 +199,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
BoostPolygon
rotated_polygon
;
BoostPolygon
rotated_polygon
;
bg
::
transform
(
minBBox
.
corners
,
rotated_polygon
,
rotate
);
bg
::
transform
(
minBBox
.
corners
,
rotated_polygon
,
rotate
);
minBBox
.
corners
=
rotated_polygon
;
minBBox
.
corners
=
rotated_polygon
;
return
true
;
}
}
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
...
@@ -438,19 +441,21 @@ const BoostPoint &Scenario::homePositon() const { return _homePosition; }
...
@@ -438,19 +441,21 @@ const BoostPoint &Scenario::homePositon() const { return _homePosition; }
bool
Scenario
::
update
()
{
bool
Scenario
::
update
()
{
if
(
!
_needsUpdate
)
if
(
!
_needsUpdate
)
return
true
;
return
true
;
bg
::
correct
(
_mArea
);
bg
::
correct
(
_sArea
);
bg
::
correct
(
_corridor
);
if
(
!
_calculateJoinedArea
())
return
false
;
if
(
!
_calculateBoundingBox
())
if
(
!
_calculateBoundingBox
())
return
false
;
return
false
;
if
(
!
_calculateTiles
())
if
(
!
_calculateTiles
())
return
false
;
return
false
;
if
(
!
_calculateJoinedArea
())
return
false
;
_needsUpdate
=
false
;
_needsUpdate
=
false
;
return
true
;
return
true
;
}
}
bool
Scenario
::
_calculateBoundingBox
()
{
bool
Scenario
::
_calculateBoundingBox
()
{
minimalBoundingBox
(
_mArea
,
_mAreaBoundingBox
);
return
minimalBoundingBox
(
_mArea
,
_mAreaBoundingBox
);
return
true
;
}
}
/**
/**
...
@@ -474,9 +479,12 @@ bool Scenario::_calculateTiles() {
...
@@ -474,9 +479,12 @@ bool Scenario::_calculateTiles() {
_tileCenterPoints
.
clear
();
_tileCenterPoints
.
clear
();
if
(
_tileWidth
<=
0
*
bu
::
si
::
meter
||
_tileHeight
<=
0
*
bu
::
si
::
meter
||
if
(
_tileWidth
<=
0
*
bu
::
si
::
meter
||
_tileHeight
<=
0
*
bu
::
si
::
meter
||
_minTileArea
<=
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
)
{
_minTileArea
<
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
)
{
errorString
=
std
::
stringstream
ss
;
"Parameters tileWidth, tileHeight, minTileArea must be positive."
;
ss
<<
"Parameters tileWidth ("
<<
_tileWidth
<<
"), tileHeight ("
<<
_tileHeight
<<
"), minTileArea ("
<<
_minTileArea
<<
") must be positive."
;
errorString
=
ss
.
str
();
return
false
;
return
false
;
}
}
...
@@ -504,9 +512,8 @@ bool Scenario::_calculateTiles() {
...
@@ -504,9 +512,8 @@ bool Scenario::_calculateTiles() {
if
(
iMax
<
1
||
jMax
<
1
)
{
if
(
iMax
<
1
||
jMax
<
1
)
{
std
::
stringstream
ss
;
std
::
stringstream
ss
;
ss
<<
"Tile width or Tile height to large. "
ss
<<
"Tile width ("
<<
_tileWidth
<<
") or tile height ("
<<
_tileHeight
<<
"tile width = "
<<
_tileWidth
<<
", "
<<
") to large for measurement area."
;
<<
"tile height = "
<<
_tileHeight
<<
"."
<<
std
::
endl
;
errorString
=
ss
.
str
();
errorString
=
ss
.
str
();
return
false
;
return
false
;
}
}
...
@@ -554,8 +561,10 @@ bool Scenario::_calculateTiles() {
...
@@ -554,8 +561,10 @@ bool Scenario::_calculateTiles() {
}
}
if
(
_tiles
.
size
()
<
1
)
{
if
(
_tiles
.
size
()
<
1
)
{
errorString
=
std
::
stringstream
ss
;
"No tiles calculated. Is the minTileArea parameter large enough?"
;
ss
<<
"No tiles calculated. Is the minTileArea ("
<<
_minTileArea
<<
") parameter large enough?"
;
errorString
=
ss
.
str
();
return
false
;
return
false
;
}
}
...
@@ -590,7 +599,21 @@ bool Scenario::_calculateJoinedArea() {
...
@@ -590,7 +599,21 @@ bool Scenario::_calculateJoinedArea() {
}
else
if
(
corridor_is_connection
)
{
}
else
if
(
corridor_is_connection
)
{
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
}
else
{
}
else
{
errorString
=
"Areas are not overlapping"
;
std
::
stringstream
ss
;
auto
printPoint
=
[
&
ss
](
const
BoostPoint
&
p
)
{
ss
<<
" ("
<<
p
.
get
<
0
>
()
<<
", "
<<
p
.
get
<
1
>
()
<<
")"
;
};
ss
<<
"Areas are not overlapping."
<<
std
::
endl
;
ss
<<
"Measurement area:"
;
bg
::
for_each_point
(
_mArea
,
printPoint
);
ss
<<
std
::
endl
;
ss
<<
"Service area:"
;
bg
::
for_each_point
(
_sArea
,
printPoint
);
ss
<<
std
::
endl
;
ss
<<
"Corridor:"
;
bg
::
for_each_point
(
_corridor
,
printPoint
);
ss
<<
std
::
endl
;
errorString
=
ss
.
str
();
return
false
;
return
false
;
}
}
...
@@ -872,21 +895,16 @@ bool flight_plan::route(const BoostPolygon &area,
...
@@ -872,21 +895,16 @@ bool flight_plan::route(const BoostPolygon &area,
BoostLineString
vertices
;
BoostLineString
vertices
;
size_t
n0
=
0
;
size_t
n0
=
0
;
for
(
const
auto
&
t
:
transects
)
{
for
(
const
auto
&
t
:
transects
)
{
if
(
t
.
size
()
>=
2
)
{
n0
+=
std
::
min
<
std
::
size_t
>
(
t
.
size
(),
2
);
n0
+=
2
;
}
else
{
n0
+=
1
;
}
}
}
vertices
.
reserve
(
n0
);
vertices
.
reserve
(
n0
);
struct
TransectInfo
{
struct
TransectInfo
{
TransectInfo
(
size_t
n
,
bool
f
)
:
index
(
n
),
front
(
f
)
{}
TransectInfo
(
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
<
TransectInfo
>
transectInfoList
;
size_t
idx
=
0
;
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
());
...
@@ -1038,6 +1056,7 @@ bool flight_plan::route(const BoostPolygon &area,
...
@@ -1038,6 +1056,7 @@ bool flight_plan::route(const BoostPolygon &area,
idx2Vertex
(
idxList
,
route
);
idx2Vertex
(
idxList
,
route
);
}
}
}
}
return
true
;
}
}
}
// namespace snake
}
// namespace snake
...
...
src/Snake/snake.h
View file @
990aa5eb
...
@@ -97,10 +97,10 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
...
@@ -97,10 +97,10 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
longitude
(),
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
longitude
(),
origin
.
altitude
(),
earth
);
origin
.
altitude
(),
earth
);
double
x
,
y
,
z
;
double
x
=
0
,
y
=
0
,
z
=
0
;
proj
.
Forward
(
in
.
latitude
(),
in
.
longitude
(),
in
.
altitude
(),
x
,
y
,
z
);
proj
.
Forward
(
in
.
latitude
(),
in
.
longitude
(),
in
.
altitude
(),
x
,
y
,
z
);
out
.
set
<
0
>
(
x
);
out
.
set
<
0
>
(
x
);
out
.
set
<
0
>
(
y
);
out
.
set
<
1
>
(
y
);
(
void
)
z
;
(
void
)
z
;
}
}
template
<
class
GeoPoint
>
template
<
class
GeoPoint
>
...
@@ -110,7 +110,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
...
@@ -110,7 +110,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
longitude
(),
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
longitude
(),
origin
.
altitude
(),
earth
);
origin
.
altitude
(),
earth
);
double
lat
,
lon
,
alt
;
double
lat
=
0
,
lon
=
0
,
alt
=
0
;
proj
.
Reverse
(
in
.
get
<
0
>
(),
in
.
get
<
1
>
(),
0
.
0
,
lat
,
lon
,
alt
);
proj
.
Reverse
(
in
.
get
<
0
>
(),
in
.
get
<
1
>
(),
0
.
0
,
lat
,
lon
,
alt
);
out
.
setLatitude
(
lat
);
out
.
setLatitude
(
lat
);
out
.
setLongitude
(
lon
);
out
.
setLongitude
(
lon
);
...
@@ -118,7 +118,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
...
@@ -118,7 +118,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
}
}
void
polygonCenter
(
const
BoostPolygon
&
polygon
,
BoostPoint
&
center
);
void
polygonCenter
(
const
BoostPolygon
&
polygon
,
BoostPoint
&
center
);
void
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
BoundingBox
&
minBBox
);
bool
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
BoundingBox
&
minBBox
);
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
double
offset
);
double
offset
);
...
@@ -242,7 +242,7 @@ bool route(const BoostPolygon &area, const Transects &transects,
...
@@ -242,7 +242,7 @@ bool route(const BoostPolygon &area, const Transects &transects,
namespace
detail
{
namespace
detail
{
const
double
offsetConstant
=
const
double
offsetConstant
=
0
.
1
;
// meter, polygon offset to compenstate for numerical inaccurracies.
0
.
5
;
// meter, polygon offset to compenstate for numerical inaccurracies.
}
// namespace detail
}
// namespace detail
}
// namespace snake
}
// namespace snake
...
...
src/Wima/Snake/SnakeDataManager.cc
View file @
990aa5eb
...
@@ -306,17 +306,26 @@ void SnakeDataManager::setMeasurementArea(
...
@@ -306,17 +306,26 @@ void SnakeDataManager::setMeasurementArea(
const
QList
<
QGeoCoordinate
>
&
measurementArea
)
{
const
QList
<
QGeoCoordinate
>
&
measurementArea
)
{
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
mArea
=
measurementArea
;
this
->
pImpl
->
mArea
=
measurementArea
;
for
(
auto
&
vertex
:
this
->
pImpl
->
mArea
)
{
vertex
.
setAltitude
(
0
);
}
}
}
void
SnakeDataManager
::
setServiceArea
(
void
SnakeDataManager
::
setServiceArea
(
const
QList
<
QGeoCoordinate
>
&
serviceArea
)
{
const
QList
<
QGeoCoordinate
>
&
serviceArea
)
{
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
sArea
=
serviceArea
;
this
->
pImpl
->
sArea
=
serviceArea
;
for
(
auto
&
vertex
:
this
->
pImpl
->
sArea
)
{
vertex
.
setAltitude
(
0
);
}
}
}
void
SnakeDataManager
::
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
)
{
void
SnakeDataManager
::
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
)
{
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
corridor
=
corridor
;
this
->
pImpl
->
corridor
=
corridor
;
for
(
auto
&
vertex
:
this
->
pImpl
->
corridor
)
{
vertex
.
setAltitude
(
0
);
}
}
}
const
QmlObjectListModel
*
SnakeDataManager
::
tiles
()
const
{
const
QmlObjectListModel
*
SnakeDataManager
::
tiles
()
const
{
...
@@ -418,9 +427,21 @@ void SnakeDataManager::disableRosBride() {
...
@@ -418,9 +427,21 @@ void SnakeDataManager::disableRosBride() {
}
}
void
SnakeDataManager
::
run
()
{
void
SnakeDataManager
::
run
()
{
#ifndef NDEBUG
auto
startTime
=
std
::
chrono
::
high_resolution_clock
::
now
();
qDebug
()
<<
"SnakeDataManager::run()"
;
#endif
this
->
pImpl
->
calcInProgress
.
store
(
true
);
this
->
pImpl
->
calcInProgress
.
store
(
true
);
emit
calcInProgressChanged
(
this
->
pImpl
->
calcInProgress
.
load
());
emit
calcInProgressChanged
(
this
->
pImpl
->
calcInProgress
.
load
());
auto
onExit
=
[
this
]
{
auto
onExit
=
[
this
,
&
startTime
]
{
#ifndef NDEBUG
qDebug
()
<<
"SnakeDataManager::run() execution time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
startTime
)
.
count
()
<<
" ms"
;
#endif
this
->
pImpl
->
calcInProgress
.
store
(
false
);
this
->
pImpl
->
calcInProgress
.
store
(
false
);
emit
calcInProgressChanged
(
this
->
pImpl
->
calcInProgress
.
load
());
emit
calcInProgressChanged
(
this
->
pImpl
->
calcInProgress
.
load
());
};
};
...
@@ -443,6 +464,8 @@ void SnakeDataManager::run() {
...
@@ -443,6 +464,8 @@ void SnakeDataManager::run() {
this
->
pImpl
->
ENUOrigin
=
this
->
pImpl
->
mArea
.
front
();
this
->
pImpl
->
ENUOrigin
=
this
->
pImpl
->
mArea
.
front
();
auto
&
origin
=
this
->
pImpl
->
ENUOrigin
;
auto
&
origin
=
this
->
pImpl
->
ENUOrigin
;
qDebug
()
<<
"SnakeDataManager::run(): origin: "
<<
origin
.
latitude
()
<<
" "
<<
origin
.
longitude
()
<<
" "
<<
origin
.
altitude
();
// Update measurement area.
// Update measurement area.
auto
&
mAreaEnu
=
this
->
pImpl
->
scenario
.
measurementArea
();
auto
&
mAreaEnu
=
this
->
pImpl
->
scenario
.
measurementArea
();
auto
&
mArea
=
this
->
pImpl
->
mArea
;
auto
&
mArea
=
this
->
pImpl
->
mArea
;
...
@@ -478,75 +501,7 @@ void SnakeDataManager::run() {
...
@@ -478,75 +501,7 @@ void SnakeDataManager::run() {
return
;
return
;
}
}
// Asynchronously update flightplan.
// Store scenario data.
std
::
string
errorString
;
auto
future
=
std
::
async
([
this
,
&
errorString
,
&
origin
]
{
snake
::
Angle
alpha
(
-
this
->
pImpl
->
scenario
.
mAreaBoundingBox
().
angle
*
degree
::
degree
);
snake
::
flight_plan
::
Transects
transects
;
transects
.
push_back
(
bg
::
model
::
linestring
<
snake
::
BoostPoint
>
{
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
)
{
this
->
pImpl
->
errorMessage
=
"Not able to generate transects."
;
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
)
{
this
->
pImpl
->
errorMessage
=
"Routing error."
;
return
value
;
}
// Store arrival path.
const
auto
&
firstWaypoint
=
transectsRouted
.
front
().
front
();
long
startIdx
=
0
;
for
(
long
i
=
0
;
i
<
long
(
route
.
size
());
++
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
firstWaypoint
)
{
startIdx
=
i
;
break
;
}
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
arrivalPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
arrivalPath
.
push_back
(
geoVertex
);
}
// Store return path.
long
endIdx
=
0
;
const
auto
&
lastWaypoint
=
transectsRouted
.
back
().
back
();
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
lastWaypoint
)
{
endIdx
=
i
;
break
;
}
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
returnPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
returnPath
.
push_back
(
geoVertex
);
}
// Store waypoints.
for
(
long
i
=
startIdx
;
i
<=
endIdx
;
++
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
waypointsENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
waypoints
.
push_back
(
geoVertex
);
}
return
true
;
});
// Continue with storing scenario data in the mean time.
{
{
// Get tiles.
// Get tiles.
const
auto
&
tiles
=
this
->
pImpl
->
scenario
.
tiles
();
const
auto
&
tiles
=
this
->
pImpl
->
scenario
.
tiles
();
...
@@ -576,12 +531,69 @@ void SnakeDataManager::run() {
...
@@ -576,12 +531,69 @@ void SnakeDataManager::run() {
}
}
}
}
future
.
wait
();
// Create transects.
// Trying to generate flight plan.
std
::
string
errorString
;
if
(
!
future
.
get
())
{
snake
::
Angle
alpha
(
-
this
->
pImpl
->
scenario
.
mAreaBoundingBox
().
angle
*
// error
degree
::
degree
);
snake
::
flight_plan
::
Transects
transects
;
transects
.
push_back
(
bg
::
model
::
linestring
<
snake
::
BoostPoint
>
{
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
)
{
this
->
pImpl
->
errorMessage
=
errorString
.
c_str
();
return
;
}
// Route transects
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
)
{
this
->
pImpl
->
errorMessage
=
errorString
.
c_str
();
this
->
pImpl
->
errorMessage
=
errorString
.
c_str
();
}
else
{
return
;
// Success!!!
}
// Store arrival path.
const
auto
&
firstWaypoint
=
transectsRouted
.
front
().
front
();
long
startIdx
=
0
;
for
(
long
i
=
0
;
i
<
long
(
route
.
size
());
++
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
firstWaypoint
)
{
startIdx
=
i
;
break
;
}
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
arrivalPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
arrivalPath
.
push_back
(
geoVertex
);
}
// Store return path.
long
endIdx
=
0
;
const
auto
&
lastWaypoint
=
transectsRouted
.
back
().
back
();
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
lastWaypoint
)
{
endIdx
=
i
;
break
;
}
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
returnPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
returnPath
.
push_back
(
geoVertex
);
}
// Store waypoints.
for
(
long
i
=
startIdx
;
i
<=
endIdx
;
++
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
waypointsENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
waypoints
.
push_back
(
geoVertex
);
}
}
}
}
src/Wima/WimaController.cc
View file @
990aa5eb
...
@@ -184,7 +184,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
...
@@ -184,7 +184,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact
*
WimaController
::
altitude
()
{
return
&
_altitude
;
}
Fact
*
WimaController
::
altitude
()
{
return
&
_altitude
;
}
QmlObjectListModel
*
WimaController
::
snakeTiles
()
{
QmlObjectListModel
*
WimaController
::
snakeTiles
()
{
return
const_cast
<
QmlObjectListModel
*>
(
this
->
_
snakeDM
.
tiles
());
return
const_cast
<
QmlObjectListModel
*>
(
this
->
_
currentDM
->
tiles
());
}
}
QVariantList
WimaController
::
snakeTileCenterPoints
()
{
QVariantList
WimaController
::
snakeTileCenterPoints
()
{
...
@@ -710,7 +710,8 @@ void WimaController::_initSmartRTL() {
...
@@ -710,7 +710,8 @@ void WimaController::_initSmartRTL() {
void
WimaController
::
_DMFinishedHandler
()
{
void
WimaController
::
_DMFinishedHandler
()
{
if
(
!
_snakeDM
.
success
())
{
if
(
!
_snakeDM
.
success
())
{
// qgcApp()->showMessage(r.errorMessage);
qDebug
()
<<
_snakeDM
.
errorMessage
();
// qgcApp()->showMessage(_snakeDM.errorMessage());
return
;
return
;
}
}
...
@@ -784,10 +785,12 @@ void WimaController::_progressChangedHandler() {
...
@@ -784,10 +785,12 @@ void WimaController::_progressChangedHandler() {
void
WimaController
::
_enableSnakeChangedHandler
()
{
void
WimaController
::
_enableSnakeChangedHandler
()
{
if
(
this
->
_enableSnake
.
rawValue
().
toBool
())
{
if
(
this
->
_enableSnake
.
rawValue
().
toBool
())
{
qDebug
()
<<
"WimaController: enabling snake."
;
this
->
_snakeDM
.
enableRosBridge
();
this
->
_snakeDM
.
enableRosBridge
();
_switchDataManager
(
_snakeDM
);
_switchDataManager
(
_snakeDM
);
_currentDM
->
start
();
_currentDM
->
start
();
}
else
{
}
else
{
qDebug
()
<<
"WimaController: disabling snake."
;
this
->
_snakeDM
.
disableRosBride
();
this
->
_snakeDM
.
disableRosBride
();
_switchDataManager
(
_emptyDM
);
_switchDataManager
(
_emptyDM
);
}
}
...
...
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