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
99e5ae38
Commit
99e5ae38
authored
Nov 10, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
addapting wima stuff to qgc master branch, code not ready
parent
5e177398
Changes
17
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
17 changed files
with
320 additions
and
702 deletions
+320
-702
SimpleMissionItem.h
src/MissionManager/SimpleMissionItem.h
+1
-1
TransectStyleComplexItem.h
src/MissionManager/TransectStyleComplexItem.h
+3
-3
CircularSurvey.cc
src/Wima/CircularSurvey.cc
+17
-99
CircularSurvey.h
src/Wima/CircularSurvey.h
+5
-18
WimaArea.cc
src/Wima/Geometry/WimaArea.cc
+1
-33
WimaMeasurementArea.cc
src/Wima/Geometry/WimaMeasurementArea.cc
+71
-62
WimaServiceArea.cc
src/Wima/Geometry/WimaServiceArea.cc
+19
-11
CircularGenerator.cpp
src/Wima/Snake/CircularGenerator.cpp
+3
-1
LinearGenerator.cpp
src/Wima/Snake/LinearGenerator.cpp
+3
-1
NemoInterface.cpp
src/Wima/Snake/NemoInterface.cpp
+31
-32
DefaultManager.cpp
src/Wima/WaypointManager/DefaultManager.cpp
+4
-4
RTLManager.cpp
src/Wima/WaypointManager/RTLManager.cpp
+3
-3
Utils.cpp
src/Wima/WaypointManager/Utils.cpp
+6
-43
Utils.h
src/Wima/WaypointManager/Utils.h
+96
-119
WimaController.cc
src/Wima/WimaController.cc
+1
-197
WimaController.h
src/Wima/WimaController.h
+0
-16
WimaPlaner.cc
src/Wima/WimaPlaner.cc
+56
-59
No files found.
src/MissionManager/SimpleMissionItem.h
View file @
99e5ae38
...
...
@@ -118,7 +118,7 @@ public:
QString
mapVisualQML
(
void
)
const
override
{
return
QStringLiteral
(
"SimpleItemMapVisual.qml"
);
}
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
final
;
v
oid
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
v
irtual
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
ReadyForSaveState
readyForSaveState
(
void
)
const
final
;
double
additionalTimeDelay
(
void
)
const
final
;
bool
exitCoordinateSameAsEntry
(
void
)
const
final
{
return
true
;
}
...
...
src/MissionManager/TransectStyleComplexItem.h
View file @
99e5ae38
...
...
@@ -88,8 +88,8 @@ public:
// Overrides from VisualMissionItem
void
save
(
QJsonArray
&
planItems
)
override
=
0
;
bool
specifiesCoordinate
(
void
)
const
override
=
0
;
v
oid
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
v
oid
applyNewAltitude
(
double
newAltitude
)
final
;
v
irtual
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
v
irtual
void
applyNewAltitude
(
double
newAltitude
)
final
;
bool
dirty
(
void
)
const
final
{
return
_dirty
;
}
bool
isSimpleItem
(
void
)
const
final
{
return
false
;
}
bool
isStandaloneCoordinate
(
void
)
const
final
{
return
false
;
}
...
...
@@ -100,7 +100,7 @@ public:
double
specifiedFlightSpeed
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalYaw
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalPitch
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
v
oid
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
v
irtual
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
ReadyForSaveState
readyForSaveState
(
void
)
const
override
;
QString
commandDescription
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
QString
commandName
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
...
...
src/Wima/CircularSurvey.cc
View file @
99e5ae38
...
...
@@ -19,6 +19,8 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY
(
CircularSurveyLog
,
"CircularSurveyLog"
)
template
<
typename
T
>
...
...
@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const
char
*
CircularSurvey
::
CircularSurveyName
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
variantName
=
"Variant"
;
CircularSurvey
::
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
CircularSurvey
::
CircularSurvey
(
PlanMasterController
*
masterController
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
QObject
*
parent
)
:
TransectStyleComplexItem
(
vehicle
,
flyView
,
settingsGroup
,
parent
),
:
TransectStyleComplexItem
(
masterController
,
flyView
,
settingsGroup
,
parent
),
_state
(
STATE
::
IDLE
),
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/CircularSurvey.SettingsGroup.json"
),
this
)),
...
...
@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&
CircularSurvey
::
_setTransects
);
connect
(
this
->
_pWorker
.
get
(),
&
RoutingThread
::
calculatingChanged
,
this
,
&
CircularSurvey
::
calculatingChanged
);
this
->
_transectsDirty
=
true
;
// Altitude
connect
(
&
_cameraCalc
,
&
CameraCalc
::
distanceToSurfaceRelativeChanged
,
this
,
&
CircularSurvey
::
coordinateHasRelativeAltitudeChanged
);
connect
(
&
_cameraCalc
,
&
CameraCalc
::
distanceToSurfaceRelativeChanged
,
this
,
&
CircularSurvey
::
exitCoordinateHasRelativeAltitudeChanged
);
// Register Generators.
auto
lg
=
std
::
make_shared
<
routing
::
LinearGenerator
>
(
this
->
_pAreaData
);
...
...
@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return
false
;
}
if
(
!
_load
(
complexObject
,
errorString
))
{
if
(
!
load
(
complexObject
,
sequenceNumber
,
errorString
))
{
_ignoreRecalc
=
false
;
return
false
;
}
...
...
@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_ignoreRecalc
=
false
;
_recalcComplexDistance
();
if
(
_cameraShots
==
0
)
{
// Shot count was possibly not available from plan file
_recalcCameraShots
();
...
...
@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
bool
CircularSurvey
::
specifiesCoordinate
()
const
{
return
true
;
}
void
CircularSurvey
::
appendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
{
if
(
_transectsDirty
)
return
;
if
(
_loadedMissionItems
.
count
())
{
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems
(
items
,
missionItemParent
);
}
else
{
// Build the mission items on the fly
_buildAndAppendMissionItems
(
items
,
missionItemParent
);
}
}
void
CircularSurvey
::
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
{
if
(
_transectsDirty
)
return
;
int
seqNum
=
_sequenceNumber
;
for
(
const
MissionItem
*
loadedMissionItem
:
_loadedMissionItems
)
{
MissionItem
*
item
=
new
MissionItem
(
*
loadedMissionItem
,
missionItemParent
);
item
->
setSequenceNumber
(
seqNum
++
);
items
.
append
(
item
);
}
}
void
CircularSurvey
::
_buildAndAppendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
{
if
(
_transectsDirty
||
_transects
.
count
()
==
0
)
return
;
MissionItem
*
item
;
int
seqNum
=
_sequenceNumber
;
MAV_FRAME
mavFrame
=
followTerrain
()
||
!
_cameraCalc
.
distanceToSurfaceRelative
()
?
MAV_FRAME_GLOBAL
:
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
for
(
const
QList
<
TransectStyleComplexItem
::
CoordInfo_t
>
&
transect
:
_transects
)
{
// bool transectEntry = true;
for
(
const
CoordInfo_t
&
transectCoordInfo
:
transect
)
{
item
=
new
MissionItem
(
seqNum
++
,
MAV_CMD_NAV_WAYPOINT
,
mavFrame
,
0
,
// Hold time (delay for hover and capture to settle vehicle
// before image is taken)
0.0
,
// No acceptance radius specified
0.0
,
// Pass through waypoint
std
::
numeric_limits
<
double
>::
quiet_NaN
(),
// Yaw unchanged
transectCoordInfo
.
coord
.
latitude
(),
transectCoordInfo
.
coord
.
longitude
(),
transectCoordInfo
.
coord
.
altitude
(),
true
,
// autoContinue
false
,
// isCurrentItem
missionItemParent
);
items
.
append
(
item
);
}
}
}
bool
CircularSurvey
::
_switchToGenerator
(
const
CircularSurvey
::
PtrGenerator
&
newG
)
{
if
(
this
->
_pGenerator
!=
newG
)
{
...
...
@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
}
void
CircularSurvey
::
_updateWorker
()
{
// Mark transects as dirty.
this
->
_transectsDirty
=
true
;
// Reset data.
this
->
_transects
.
clear
();
this
->
_variantVector
.
clear
();
...
...
@@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() {
}
}
void
CircularSurvey
::
applyNewAltitude
(
double
newAltitude
)
{
_cameraCalc
.
valueSetIsDistance
()
->
setRawValue
(
true
);
_cameraCalc
.
distanceToSurface
()
->
setRawValue
(
newAltitude
);
_cameraCalc
.
setDistanceToSurfaceRelative
(
true
);
}
double
CircularSurvey
::
timeBetweenShots
()
{
return
1
;
}
QString
CircularSurvey
::
commandDescription
()
const
{
...
...
@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString
CircularSurvey
::
abbreviation
()
const
{
return
tr
(
"C.S."
);
}
bool
CircularSurvey
::
readyForSave
()
const
{
return
TransectStyleComplexItem
::
readyForSave
()
&&
!
_transectsDirty
&&
this
->
_state
==
STATE
::
IDLE
;
TransectStyleComplexItem
::
ReadyForSaveState
CircularSurvey
::
readyForSaveState
()
const
{
if
(
TransectStyleComplexItem
::
readyForSaveState
()
==
TransectStyleComplexItem
::
ReadyForSaveState
::
ReadyForSave
)
{
if
(
this
->
_state
==
STATE
::
IDLE
)
{
return
ReadyForSaveState
::
ReadyForSave
;
}
else
{
return
ReadyForSaveState
::
NotReadyForSaveData
;
}
}
else
{
return
TransectStyleComplexItem
::
readyForSaveState
();
}
}
double
CircularSurvey
::
additionalTimeDelay
()
const
{
return
0
;
}
...
...
@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
<<
" ms"
;
}
void
CircularSurvey
::
_recalcComplexDistance
()
{
_complexDistance
=
0
;
if
(
_transectsDirty
)
return
;
for
(
int
i
=
0
;
i
<
_visualTransectPoints
.
count
()
-
1
;
i
++
)
{
_complexDistance
+=
_visualTransectPoints
[
i
].
value
<
QGeoCoordinate
>
().
distanceTo
(
_visualTransectPoints
[
i
+
1
].
value
<
QGeoCoordinate
>
());
}
emit
complexDistanceChanged
();
}
// no cameraShots in Circular Survey, add if desired
void
CircularSurvey
::
_recalcCameraShots
()
{
_cameraShots
=
0
;
}
...
...
@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
&
CircularSurvey
::
_changeVariant
);
this
->
_changeVariantWorker
();
// Mark transect ready.
this
->
_transectsDirty
=
false
;
this
->
_state
=
STATE
::
SKIPP
;
this
->
_rebuildTransects
();
}
else
{
...
...
src/Wima/CircularSurvey.h
View file @
99e5ae38
...
...
@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
using
PtrGenerator
=
std
::
shared_ptr
<
routing
::
GeneratorBase
>
;
using
PtrRoutingData
=
std
::
shared_ptr
<
RoutingData
>
;
using
PtrWorker
=
std
::
shared_ptr
<
RoutingThread
>
;
using
PtrWorker
=
std
::
unique_ptr
<
RoutingThread
>
;
using
Transects
=
QList
<
QList
<
CoordInfo_t
>>
;
using
Variant
=
Transects
;
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default
/// polygon
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
CircularSurvey
(
PlanMasterController
*
masterController
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
QObject
*
parent
);
~
CircularSurvey
();
...
...
@@ -63,14 +60,11 @@ public:
QString
mapVisualQML
(
void
)
const
override
final
;
void
save
(
QJsonArray
&
planItems
)
override
final
;
bool
specifiesCoordinate
(
void
)
const
override
final
;
void
appendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
override
final
;
void
applyNewAltitude
(
double
newAltitude
)
override
final
;
double
timeBetweenShots
(
void
)
override
final
;
QString
commandDescription
(
void
)
const
override
final
;
QString
commandName
(
void
)
const
override
final
;
QString
abbreviation
(
void
)
const
override
final
;
bool
readyForSav
e
(
void
)
const
override
final
;
ReadyForSaveState
readyForSaveStat
e
(
void
)
const
override
final
;
double
additionalTimeDelay
(
void
)
const
override
final
;
// Generator
...
...
@@ -97,7 +91,6 @@ signals:
private
slots
:
// Overrides from TransectStyleComplexItem
void
_rebuildTransectsPhase1
(
void
)
final
;
void
_recalcComplexDistance
(
void
)
final
;
void
_recalcCameraShots
(
void
)
final
;
// Worker functions.
...
...
@@ -108,11 +101,6 @@ private slots:
void
_reverseWorker
();
private:
void
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
void
_buildAndAppendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
bool
_switchToGenerator
(
const
PtrGenerator
&
newG
);
// State.
...
...
@@ -139,7 +127,6 @@ private:
PtrGenerator
_pGenerator
;
// Routing.
using
Variant
=
Transects
;
QVector
<
Variant
>
_variantVector
;
PtrWorker
_pWorker
;
};
src/Wima/Geometry/WimaArea.cc
View file @
99e5ae38
...
...
@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
}
else
if
(
retValue
==
JoinPolygonError
::
PathSizeLow
)
{
qWarning
(
"Polygon vertex count is low."
);
}
else
{
Q
Vector
<
QGeoCoordinate
>
path
;
Q
List
<
QGeoCoordinate
>
path
;
toGeoList
(
joinedPolygon
,
origin
,
path
);
// qWarning("after transform");
// qWarning() << path;
...
...
@@ -491,38 +491,6 @@ void WimaArea::init() {
&
WimaArea
::
recalcInteractivity
);
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the console.
*/
void
print
(
const
WimaArea
&
area
)
{
QString
message
;
print
(
area
,
message
);
qWarning
()
<<
message
;
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the \a outputString.
*/
void
print
(
const
WimaArea
&
area
,
QString
&
outputString
)
{
outputString
.
append
(
QString
(
"Type: %1
\n
"
).
arg
(
area
.
objectName
()));
print
(
static_cast
<
const
QGCMapPolygon
&>
(
area
),
outputString
);
outputString
.
append
(
QString
(
"Maximum Altitude: %1
\n
"
).
arg
(
area
.
_maxAltitude
));
outputString
.
append
(
QString
(
"Border Polygon Offset: %1
\n
"
)
.
arg
(
area
.
_borderPolygonOffset
.
rawValue
().
toDouble
()));
outputString
.
append
(
QString
(
"Border Polygon Coordinates
\n
"
)
.
arg
(
area
.
_borderPolygonOffset
.
rawValue
().
toDouble
()));
for
(
int
i
=
0
;
i
<
area
.
_borderPolygon
.
count
();
i
++
)
{
QGeoCoordinate
coordinate
=
area
.
_borderPolygon
.
vertexCoordinate
(
i
);
outputString
.
append
(
QString
(
"%1
\n
"
).
arg
(
coordinate
.
toString
(
QGeoCoordinate
::
Degrees
)));
}
}
// QDoc Documentation
/*!
...
...
src/Wima/Geometry/WimaMeasurementArea.cc
View file @
99e5ae38
...
...
@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData
&
TileData
::
operator
=
(
const
TileData
&
other
)
{
this
->
tiles
.
clearAndDeleteContents
();
for
(
std
::
size_t
i
=
0
;
i
<
std
::
size_t
(
other
.
tiles
.
count
());
++
i
)
{
const
auto
*
obj
=
other
.
tiles
.
get
(
i
)
;
const
auto
*
obj
=
other
.
tiles
[
i
]
;
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
this
->
tiles
.
append
(
new
SnakeTile
(
*
tile
,
this
));
...
...
@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
}
bool
TileData
::
operator
==
(
const
TileData
&
other
)
const
{
return
this
->
tiles
==
other
.
tiles
&&
this
->
tileCenterPoints
==
other
.
tileCenterPoints
;
if
(
this
->
tileCenterPoints
==
other
.
tileCenterPoints
&&
this
->
tiles
.
count
()
==
other
.
tiles
.
count
())
{
for
(
int
i
=
0
;
i
<
other
.
tiles
.
count
();
++
i
)
{
if
(
this
->
tiles
[
i
]
!=
other
.
tiles
[
i
])
{
return
false
;
}
}
return
true
;
}
else
{
return
false
;
}
}
bool
TileData
::
operator
!=
(
const
TileData
&
other
)
const
{
...
...
@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this
/* QObject parent */
)),
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
minTileAreaName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
this
/* QObject parent */
)),
_state
(
STATE
::
IDLE
)
{
...
...
@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this
/* QObject parent */
)),
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
minTileAreaName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
this
/* QObject parent */
)),
_state
(
STATE
::
IDLE
)
{
...
...
@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void
WimaMeasurementArea
::
saveToJson
(
QJsonObject
&
json
)
{
if
(
ready
())
{
this
->
WimaArea
::
saveToJson
(
json
);
json
[
tileHeightName
]
=
_tileHeight
.
rawValue
().
toDouble
();
json
[
tileWidthName
]
=
_tileWidth
.
rawValue
().
toDouble
();
this
->
WimaArea
::
saveToJson
(
json
);
json
[
tileHeightName
]
=
_tileHeight
.
rawValue
().
toDouble
();
json
[
tileWidthName
]
=
_tileWidth
.
rawValue
().
toDouble
();
json
[
minTileAreaName
]
=
_minTileAreaPercent
.
rawValue
().
toDouble
();
json
[
showTilesName
]
=
_showTiles
.
rawValue
().
toBool
();
json
[
areaTypeName
]
=
WimaMeasurementAreaName
;
json
[
showTilesName
]
=
_showTiles
.
rawValue
().
toBool
();
json
[
areaTypeName
]
=
WimaMeasurementAreaName
;
}
else
{
qCDebug
(
WimaMeasurementAreaLog
)
<<
"saveToJson(): not ready for saveing."
;
}
...
...
@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
if
(
this
->
_state
!=
STATE
::
UPDATEING
&&
this
->
_state
!=
STATE
::
STOP
)
{
const
auto
height
=
this
->
_tileHeight
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
width
=
this
->
_tileWidth
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
tileArea
=
width
*
height
;
const
auto
totalArea
=
this
->
area
()
*
si
::
meter
*
si
::
meter
;
const
auto
estNumTiles
=
totalArea
/
tileArea
;
const
auto
height
=
this
->
_tileHeight
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
width
=
this
->
_tileWidth
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
tileArea
=
width
*
height
;
const
auto
totalArea
=
this
->
area
()
*
si
::
meter
*
si
::
meter
;
const
auto
estNumTiles
=
totalArea
/
tileArea
;
// Check some conditions.
if
(
long
(
std
::
ceil
(
estNumTiles
.
value
()))
<=
SNAKE_MAX_TILES
&&
this
->
count
()
>=
3
&&
this
->
isSimplePolygon
())
{
this
->
count
()
>=
3
&&
this
->
isSimplePolygon
())
{
setState
(
STATE
::
UPDATEING
);
auto
polygon
=
this
->
coordinateList
();
for
(
auto
&
v
:
polygon
)
{
v
.
setAltitude
(
0
);
}
const
auto
minArea
=
auto
polygon
=
this
->
coordinateList
();
for
(
auto
&
v
:
polygon
)
{
v
.
setAltitude
(
0
);
}
const
auto
minArea
=
this
->
_minTileAreaPercent
.
rawValue
().
toDouble
()
/
100
*
tileArea
;
auto
*
th
=
this
->
thread
();
auto
future
=
QtConcurrent
::
run
([
polygon
,
th
,
height
,
width
,
minArea
]
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
DataPtr
pData
(
new
TileData
());
// Convert to ENU system.
QGeoCoordinate
origin
=
polygon
.
first
();
FPolygon
polygonENU
;
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
vector
<
FPolygon
>
tilesENU
;
BoundingBox
bbox
;
std
::
string
errorString
;
// Generate tiles.
if
(
snake
::
tiles
(
polygonENU
,
height
,
width
,
minArea
,
tilesENU
,
bbox
,
errorString
))
{
// Convert to geo system.
for
(
const
auto
&
t
:
tilesENU
)
{
auto
geoTile
=
new
SnakeTile
(
pData
.
get
());
for
(
const
auto
&
v
:
t
.
outer
())
{
QGeoCoordinate
geoVertex
;
fromENU
(
origin
,
v
,
geoVertex
);
geoTile
->
push_back
(
geoVertex
);
auto
*
th
=
this
->
thread
();
auto
future
=
QtConcurrent
::
run
([
polygon
,
th
,
height
,
width
,
minArea
]
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
DataPtr
pData
(
new
TileData
());
// Convert to ENU system.
QGeoCoordinate
origin
=
polygon
.
first
();
FPolygon
polygonENU
;
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
vector
<
FPolygon
>
tilesENU
;
BoundingBox
bbox
;
std
::
string
errorString
;
// Generate tiles.
if
(
snake
::
tiles
(
polygonENU
,
height
,
width
,
minArea
,
tilesENU
,
bbox
,
errorString
))
{
// Convert to geo system.
for
(
const
auto
&
t
:
tilesENU
)
{
auto
geoTile
=
new
SnakeTile
(
pData
.
get
());
for
(
const
auto
&
v
:
t
.
outer
())
{
QGeoCoordinate
geoVertex
;
fromENU
(
origin
,
v
,
geoVertex
);
geoTile
->
push_back
(
geoVertex
);
}
pData
->
tiles
.
append
(
geoTile
);
// Calculate center.
snake
::
FPoint
center
;
snake
::
polygonCenter
(
t
,
center
);
QGeoCoordinate
geoCenter
;
fromENU
(
origin
,
center
,
geoCenter
);
pData
->
tileCenterPoints
.
append
(
QVariant
::
fromValue
(
geoCenter
));
}
pData
->
tiles
.
append
(
geoTile
);
// Calculate center.
snake
::
FPoint
center
;
snake
::
polygonCenter
(
t
,
center
);
QGeoCoordinate
geoCenter
;
fromENU
(
origin
,
center
,
geoCenter
);
pData
->
tileCenterPoints
.
append
(
QVariant
::
fromValue
(
geoCenter
));
}
}
pData
->
moveToThread
(
th
);
pData
->
moveToThread
(
th
);
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): update time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
.
count
()
<<
" ms"
;
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): update time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
.
count
()
<<
" ms"
;
return
pData
;
});
// QtConcurrent::run()
return
pData
;
});
// QtConcurrent::run()
this
->
_watcher
.
setFuture
(
future
);
}
this
->
_watcher
.
setFuture
(
future
);
}
}
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): execution time: "
...
...
src/Wima/Geometry/WimaServiceArea.cc
View file @
99e5ae38
#include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY
(
WimaServiceAreaLog
,
"WimaServiceAreaLog"
)
const
char
*
WimaServiceArea
::
wimaServiceAreaName
=
"Service Area"
;
const
char
*
WimaServiceArea
::
depotLatitudeName
=
"DepotLatitude"
;
const
char
*
WimaServiceArea
::
depotLongitudeName
=
"DepotLongitude"
;
...
...
@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
}
else
if
(
this
->
_depot
.
isValid
())
{
// Use nearest coordinate.
auto
minDist
=
std
::
numeric_limits
<
double
>::
infinity
();
auto
minIt
=
this
->
pathReference
().
begin
();
for
(
auto
it
=
this
->
pathReference
().
begin
();
it
<
this
->
pathReference
().
end
();
++
it
)
{
const
auto
vertex
=
it
->
value
<
QGeoCoordinate
>
();
auto
d
=
vertex
.
distanceTo
(
this
->
_depot
);
if
(
d
<
minDist
)
{
minDist
=
d
;
minIt
=
it
;
int
minIndex
=
0
;
for
(
int
idx
=
0
;
idx
<
this
->
pathModel
().
count
();
++
idx
)
{
const
QObject
*
obj
=
this
->
pathModel
()[
idx
];
const
auto
*
vertex
=
qobject_cast
<
const
QGeoCoordinate
*>
(
obj
);
if
(
vertex
!=
nullptr
)
{
auto
d
=
vertex
->
distanceTo
(
this
->
_depot
);
if
(
d
<
minDist
)
{
minDist
=
d
;
minIndex
=
idx
;
}
}
else
{
qCCritical
(
WimaServiceAreaLog
)
<<
"init(): nullptr catched!"
;
}
}
this
->
setDepot
(
minIt
->
value
<
QGeoCoordinate
>
(
));
}
else
if
(
this
->
path
Reference
().
size
()
>
0
)
{
this
->
setDepot
(
*
this
->
pathModel
().
value
<
QGeoCoordinate
*>
(
minIndex
));
}
else
if
(
this
->
path
Model
().
count
()
>
0
)
{
// Use first coordinate.
this
->
setDepot
(
this
->
pathReference
().
value
(
0
).
value
<
QGeoCoordinate
>
(
));
this
->
setDepot
(
*
this
->
pathModel
().
value
<
QGeoCoordinate
*>
(
0
));
}
}
});
...
...
src/Wima/Snake/CircularGenerator.cpp
View file @
99e5ae38
...
...
@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) {
if
(
progress
.
size
()
==
tiles
->
count
())
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
if
(
progress
[
i
]
==
100
)
{
const
auto
*
tile
=
tiles
->
value
<
const
SnakeTile
*>
(
i
);
const
auto
*
obj
=
(
*
tiles
)[
int
(
i
)];
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
snake
::
FPolygon
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
...
...
src/Wima/Snake/LinearGenerator.cpp
View file @
99e5ae38
...
...
@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
if
(
progress
.
size
()
==
tiles
->
count
())
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
if
(
progress
[
i
]
==
100
)
{
const
auto
*
tile
=
tiles
->
value
<
const
SnakeTile
*>
(
i
);
const
QObject
*
obj
=
(
*
tiles
)[
int
(
i
)];
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
snake
::
FPolygon
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
...
...
src/Wima/Snake/NemoInterface.cpp
View file @
99e5ae38
...
...
@@ -97,7 +97,7 @@ private:
};
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
StatusMap
statusMap
{
static
StatusMap
statusMap
{
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
NOT_CONNECTED
,
"Not Connected"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
...
...
@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
if
(
ros_bridge
::
isValidConnectionString
(
connectionString
.
toLocal8Bit
().
data
()))
{
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
}
else
{
QString
defaultString
(
"localhost:9090"
);
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
connectionString
);
qgcApp
()
->
showMessage
(
"Resetting connection string to: "
+
defaultString
);
connectionStringFact
->
setRawValue
(
QVariant
(
defaultString
));
// calls this function recursively
qgcApp
()
->
warningMessageBoxOnMainThread
(
"Nemo Interface"
,
"Websocket connection string possibly invalid: "
+
connectionString
+
". Trying to connect anyways."
);
}
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
};
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
...
...
@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
UniqueLock
lk1
(
this
->
ENUOriginMutex
,
std
::
adopt_lock
);
UniqueLock
lk2
(
this
->
tilesENUMutex
,
std
::
adopt_lock
);
const
auto
*
obj
=
tileData
.
tiles
.
get
(
0
)
;
const
auto
*
obj
=
tileData
.
tiles
[
0
]
;
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
if
(
tile
->
coordinateList
().
size
()
>
0
)
{
if
(
tile
->
coordinateList
().
first
().
isValid
())
{
this
->
ENUOrigin
=
tile
->
coordinateList
().
first
();
const
auto
&
origin
=
this
->
ENUOrigin
;
this
->
tilesENU
.
polygons
().
clear
();
for
(
int
i
=
0
;
i
<
tileData
.
tiles
.
count
();
++
i
)
{
obj
=
tileData
.
tiles
.
get
(
i
)
;
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
SnakeTileLocal
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
.
path
());
this
->
tilesENU
.
polygons
().
push_back
(
std
::
move
(
tileENU
));
}
else
{
this
->
ENUOrigin
=
tile
->
coordinateList
().
first
();
const
auto
&
origin
=
this
->
ENUOrigin
;
this
->
tilesENU
.
polygons
().
clear
();
for
(
int
i
=
0
;
i
<
tileData
.
tiles
.
count
();
++
i
)
{
obj
=
tileData
.
tiles
[
i
]
;
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
SnakeTileLocal
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
.
path
());
this
->
tilesENU
.
polygons
().
push_back
(
std
::
move
(
tileENU
));
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): nullptr."
;
break
;
break
;
}
}
}
}
else
{
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): Origin invalid."
;
}
}
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): tile empty."
;
}
}
}
}
else
{
this
->
tileData
.
clear
();
...
...
@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
progressMsg
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
progressMsg
.
progress
().
clear
();
qgcApp
()
->
showMessage
(
"Invalid progress message received."
);
qgcApp
()
->
informationMessageBoxOnMainThread
(
"Nemo Interface"
,
"Invalid progress message received."
);
}
emit
this
->
parent
->
progressChanged
();
...
...
@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if
(
geographic_msgs
::
geo_point
::
toJson
(
origin
,
jOrigin
,
pDoc
->
GetAllocator
()))
{
lk
.
unlock
();
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
}
else
{
lk
.
unlock
();
qCWarning
(
NemoInterfaceLog
)
...
...
@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if
(
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
jSnakeTiles
,
pDoc
->
GetAllocator
()))
{
lk
.
unlock
();
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
}
else
{
lk
.
unlock
();
qCWarning
(
NemoInterfaceLog
)
...
...
@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
this
->
pRosBridge
->
connected
()
&&
!
this
->
topicServiceSetupDone
)
{
this
->
doTopicServiceSetup
();
this
->
topicServiceSetupDone
=
true
;
this
->
topicServiceSetupDone
=
true
;