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
22c1b55e
Commit
22c1b55e
authored
Jan 19, 2021
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
temp
parent
5beee98a
Changes
25
Hide whitespace changes
Inline
Side-by-side
Showing
25 changed files
with
1534 additions
and
337 deletions
+1534
-337
qgroundcontrol.pro
qgroundcontrol.pro
+8
-3
MeasurementComplexItem.cc
src/MeasurementComplexItem/MeasurementComplexItem.cc
+73
-0
MeasurementComplexItem.h
src/MeasurementComplexItem/MeasurementComplexItem.h
+3
-0
NemoInterface.cpp
src/MeasurementComplexItem/NemoInterface.cpp
+932
-208
NemoInterface.h
src/MeasurementComplexItem/NemoInterface.h
+25
-4
GeoArea.cc
src/MeasurementComplexItem/geometry/GeoArea.cc
+1
-14
MeasurementArea.cc
src/MeasurementComplexItem/geometry/MeasurementArea.cc
+79
-50
MeasurementArea.h
src/MeasurementComplexItem/geometry/MeasurementArea.h
+3
-3
ProgressArray.h
src/MeasurementComplexItem/geometry/ProgressArray.h
+5
-2
CommandDispatcher.h
...MeasurementComplexItem/nemo_interface/CommandDispatcher.h
+0
-31
FutureWatcher.h
src/MeasurementComplexItem/nemo_interface/FutureWatcher.h
+74
-0
FutureWatcherInterface.cpp
...mentComplexItem/nemo_interface/FutureWatcherInterface.cpp
+5
-0
FutureWatcherInterface.h
...rementComplexItem/nemo_interface/FutureWatcherInterface.h
+23
-0
MeasurementTile.cpp
...MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
+6
-1
MeasurementTile.h
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
+5
-1
Task.cpp
src/MeasurementComplexItem/nemo_interface/Task.cpp
+8
-4
Task.h
src/MeasurementComplexItem/nemo_interface/Task.h
+4
-10
TaskDispatcher.cpp
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp
+111
-0
TaskDispatcher.h
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h
+70
-0
RosBridgeClient.cpp
src/comm/ros_bridge/include/RosBridgeClient.cpp
+2
-1
heartbeat.h
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+0
-1
labeled_progress.h
.../ros_bridge/include/messages/nemo_msgs/labeled_progress.h
+3
-3
progress_array.cpp
.../ros_bridge/include/messages/nemo_msgs/progress_array.cpp
+5
-0
progress_array.h
...mm/ros_bridge/include/messages/nemo_msgs/progress_array.h
+88
-0
tile.h
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
+1
-1
No files found.
qgroundcontrol.pro
View file @
22c1b55e
...
...
@@ -35,7 +35,7 @@ DebugBuild {
#
DEFINES
+=
SNAKE_SHOW_TIME
#
DEFINES
+=
SNAKE_DEBUG
#
DEFINES
+=
SNAKE_SHOW_TIME
DEFINES
+=
ROS_BRIDGE_DEBUG
#
DEFINES
+=
ROS_BRIDGE_DEBUG
}
else
{
DESTDIR
=
$$
{
OUT_PWD
}
/
release
...
...
@@ -453,10 +453,12 @@ HEADERS += \
src
/
MeasurementComplexItem
/
geometry
/
TileDiff
.
h
\
src
/
MeasurementComplexItem
/
geometry
/
geometry
.
h
\
src
/
MeasurementComplexItem
/
HashFunctions
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
FutureWatcher
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
FutureWatcherInterface
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
tileHelper
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
CommandDispatcher
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
h
\
src
/
QmlControls
/
QmlUnitsConversion
.
h
\
src
/
MeasurementComplexItem
/
geometry
/
GeoArea
.
h
\
...
...
@@ -510,6 +512,7 @@ HEADERS += \
src
/
comm
/
ros_bridge
/
include
/
message_traits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
h
\
src
/
comm
/
ros_bridge
/
include
/
server
.
h
\
...
...
@@ -529,9 +532,10 @@ SOURCES += \
src
/
MeasurementComplexItem
/
geometry
/
SafeArea
.
cc
\
src
/
MeasurementComplexItem
/
geometry
/
geometry
.
cpp
\
src
/
MeasurementComplexItem
/
HashFunctions
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
CommandDispatcher
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
FutureWatcherInterface
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
cpp
\
src
/
Vehicle
/
VehicleEscStatusFactGroup
.
cc
\
src
/
MeasurementComplexItem
/
AreaData
.
cc
\
src
/
api
/
QGCCorePlugin
.
cc
\
...
...
@@ -553,6 +557,7 @@ SOURCES += \
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
time
.
cpp
\
...
...
src/MeasurementComplexItem/MeasurementComplexItem.cc
View file @
22c1b55e
...
...
@@ -2,6 +2,7 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
#include "NemoInterface.h"
#include "RoutingThread.h"
#include "geometry/GenericCircle.h"
#include "geometry/MeasurementArea.h"
...
...
@@ -128,6 +129,10 @@ MeasurementComplexItem::MeasurementComplexItem(
resetGenerators
();
startEditing
();
// connect to nemo interface
connect
(
pNemoInterface
,
&
NemoInterface
::
progressChanged
,
this
,
&
MeasurementComplexItem
::
_onNewProgress
);
}
MeasurementComplexItem
::~
MeasurementComplexItem
()
{}
...
...
@@ -919,6 +924,69 @@ void MeasurementComplexItem::_reverseRoute() {
}
}
void
MeasurementComplexItem
::
_syncTiles
()
{
auto
areaArray
=
_pAreaData
->
measurementAreaArray
();
bool
clear
=
false
;
if
(
areaArray
.
size
()
>
0
)
{
// create tile ptr array
TilePtrArray
tilePtrArray
;
auto
*
pMeasurementArea
=
areaArray
[
0
];
auto
pTiles
=
pMeasurementArea
->
tiles
();
for
(
int
i
=
0
;
i
<
pTiles
->
count
();
++
i
)
{
auto
*
tile
=
pTiles
->
value
<
MeasurementTile
*>
(
i
);
Q_ASSERT
(
tile
!=
nullptr
);
tilePtrArray
.
push_back
(
tile
);
}
if
(
tilePtrArray
.
size
()
>
0
)
{
// create id array
IDArray
idArray
;
for
(
const
auto
*
pTile
:
tilePtrArray
)
{
idArray
.
push_back
(
pTile
->
id
());
}
// sync. necessary?
bool
doSync
=
false
;
auto
contains
=
pNemoInterface
->
containsTiles
(
idArray
);
for
(
auto
&&
logical
:
contains
)
{
if
(
logical
==
false
)
{
doSync
=
true
;
break
;
}
}
if
(
doSync
)
{
if
(
!
pNemoInterface
->
empty
())
{
(
void
)
pNemoInterface
->
clearTiles
();
}
(
void
)
pNemoInterface
->
addTiles
(
tilePtrArray
);
return
;
}
}
else
{
clear
=
true
;
}
}
else
{
clear
=
true
;
}
if
(
clear
)
{
if
(
!
pNemoInterface
->
empty
())
{
(
void
)
pNemoInterface
->
clearTiles
();
}
}
}
void
MeasurementComplexItem
::
_onNewProgress
(
const
ProgressArray
&
array
)
{
auto
areaArray
=
this
->
_pAreaData
->
measurementAreaArray
();
if
(
areaArray
.
size
()
>
0
)
{
for
(
auto
&
area
:
areaArray
)
{
area
->
updateProgress
(
array
);
}
}
}
ComplexMissionItem
::
ReadyForSaveState
MeasurementComplexItem
::
readyForSaveState
()
const
{
if
(
idle
())
{
...
...
@@ -1145,6 +1213,10 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
_updateRoute
();
}
if
(
correct
&&
isDifferent
)
{
_syncTiles
();
}
return
updated
;
}
return
false
;
...
...
@@ -1153,6 +1225,7 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
void
MeasurementComplexItem
::
abortEditing
()
{
if
(
editing
())
{
_setAreaData
(
_pAreaData
);
_syncTiles
();
_setState
(
STATE
::
IDLE
);
}
}
...
...
src/MeasurementComplexItem/MeasurementComplexItem.h
View file @
22c1b55e
...
...
@@ -10,6 +10,7 @@
#include "SettingsFact.h"
#include "AreaData.h"
#include "ProgressArray.h"
class
RoutingThread
;
class
RoutingResult
;
...
...
@@ -238,6 +239,8 @@ private slots:
void
_updateRoute
();
void
_changeVariantIndex
();
void
_reverseRoute
();
void
_syncTiles
();
void
_onNewProgress
(
const
ProgressArray
&
array
);
private:
bool
_setGenerator
(
PtrGenerator
newG
);
...
...
src/MeasurementComplexItem/NemoInterface.cpp
View file @
22c1b55e
...
...
@@ -14,9 +14,12 @@
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "nemo_interface/
CommandDisp
atcher.h"
#include "nemo_interface/
FutureW
atcher.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.h"
#include "ros_bridge/include/messages/nemo_msgs/progress_array.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
...
...
@@ -29,14 +32,23 @@
QGC_LOGGING_CATEGORY
(
NemoInterfaceLog
,
"NemoInterfaceLog"
)
#define
EVENT_TIMER_INTERVAL 100
// ms
#define NO_HEARTBEAT_TIMEOUT
5000
// ms
auto
constexpr
static
maxResponseTime
=
std
::
chrono
::
milliseconds
(
10000
);
using
hrc
=
std
::
chrono
::
high_resolution_clock
;
#define
SYNC_INTERVAL 1000
// ms
#define NO_HEARTBEAT_TIMEOUT
10000
// ms
#define CONNECTION_TIMER_INTERVAL 1000 // ms
static
constexpr
auto
maxResponseTime
=
std
::
chrono
::
milliseconds
(
10000
)
;
using
hrc
=
std
::
chrono
::
high_resolution_clock
;
using
ROSBridgePtr
=
std
::
shared_ptr
<
RosbridgeWsClient
>
;
using
UniqueLock
=
std
::
unique_lock
<
std
::
shared_timed_mutex
>
;
using
SharedLock
=
std
::
shared_lock
<
std
::
shared_timed_mutex
>
;
typedef
ros_bridge
::
messages
::
nemo_msgs
::
tile
::
GenericTile
<
QGeoCoordinate
,
QList
>
Tile
;
typedef
std
::
map
<
std
::
int64_t
,
std
::
shared_ptr
<
Tile
>>
TileMap
;
typedef
std
::
map
<
std
::
int64_t
,
std
::
shared_ptr
<
const
Tile
>>
TileMapConst
;
typedef
ros_bridge
::
messages
::
nemo_msgs
::
heartbeat
::
Heartbeat
Heartbeat
;
typedef
nemo_interface
::
TaskDispatcher
Dispatcher
;
typedef
nemo_interface
::
FutureWatcher
<
QVariant
,
std
::
shared_future
>
FutureWatcher
;
class
NemoInterface
::
Impl
{
enum
class
STATE
{
...
...
@@ -44,8 +56,11 @@ class NemoInterface::Impl {
START_BRIDGE
,
WEBSOCKET_DETECTED
,
TRY_TOPIC_SERVICE_SETUP
,
SYNC_USER
,
SYNC_SYS
,
READY
,
TIMEOUT
WEBSOCKET_TIMEOUT
,
HEARTBEAT_TIMEOUT
};
public:
...
...
@@ -57,9 +72,9 @@ public:
// Tile editing.
// Functions that require communication to device.
std
::
future
<
QVariant
>
addTiles
(
const
Tile
Array
&
tileArray
);
void
removeTiles
(
const
IDArray
&
idArray
);
void
clearTiles
();
std
::
shared_future
<
QVariant
>
addTiles
(
const
TilePtr
Array
&
tileArray
);
std
::
shared_future
<
QVariant
>
removeTiles
(
const
IDArray
&
idArray
);
std
::
shared_future
<
QVariant
>
clearTiles
();
// Functions that don't require communication to device.
TileArray
getTiles
(
const
IDArray
&
idArray
);
...
...
@@ -79,51 +94,69 @@ public:
const
QString
&
infoString
();
const
QString
&
warningString
();
void
_addTilesLocal
(
const
TileArray
&
tileArray
);
void
_removeTilesLocal
(
const
IDArray
&
idArray
);
void
_clearTilesLocal
();
void
_updateProgress
(
ProgressArray
progressArray
);
void
_setHeartbeat
(
const
QNemoHeartbeat
&
hb
);
void
_setInfoString
(
const
QString
&
info
);
void
_setWarningString
(
const
QString
&
warning
);
private:
typedef
std
::
chrono
::
time_point
<
std
::
chrono
::
high_resolution_clock
>
TimePoint
;
typedef
std
::
map
<
long
,
MeasurementTile
>
TileMap
;
typedef
ros_bridge
::
messages
::
nemo_msgs
::
heartbeat
::
Heartbeat
Heartbeat
;
typedef
nemo_interface
::
CommandDispatcher
Dispatcher
;
void
_doTopicServiceSetup
();
void
_doAction
();
// does action according to state
void
_doAction
();
void
_trySynchronize
();
bool
_isSynchronized
();
bool
_userSync
();
// thread safe
bool
_sysSync
();
// thread safe
void
_onFutureWatcherFinished
();
// thread safe
// called from dispatcher thread!
QVariant
_callAddTiles
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
);
// called from dispatcher thread!
QVariant
_callRemoveTiles
(
std
::
shared_ptr
<
IDArray
>
pIdArray
);
// called from dispatcher thread!
QVariant
_callClearTiles
();
// called from dispatcher thread!
QVariant
_callGetProgress
(
std
::
shared_ptr
<
IDArray
>
pIdArray
);
void
_addTilesRemote
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
);
void
_addTilesRemote
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
pTileArray
);
void
_removeTilesRemote
(
std
::
shared_ptr
<
IDArray
>
idArray
);
void
_clearTilesRemote
();
void
_updateProgress
(
std
::
shared_ptr
<
ProgressArray
>
pArray
);
void
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
hb
);
void
_setInfoString
(
const
QString
&
info
);
void
_setWarningString
(
const
QString
&
warning
);
bool
_setState
(
STATE
s
);
// not thread safe
bool
_setState
(
STATE
newState
);
// not thread safe
static
bool
_ready
(
STATE
s
);
static
bool
_userSync
(
STATE
s
);
static
bool
_sysSync
(
STATE
s
);
static
bool
_running
(
STATE
s
);
static
void
_translate
(
STATE
state
,
NemoInterface
::
STATUS
&
status
);
static
void
_translate
(
Heartbeat
hb
,
STATE
&
state
);
static
NemoInterface
::
STATUS
_status
(
STATE
state
);
static
QString
_toString
(
STATE
s
);
static
QString
_toString
(
NemoInterface
::
STATUS
s
);
std
::
atomic
<
STATE
>
_state
;
ROSBridgePtr
_pRosBridge
;
TileMap
_tileMap
;
NemoInterface
*
_parent
;
TileMap
_remoteTiles
;
TileMapConst
_localTiles
;
NemoInterface
*
const
_parent
;
Dispatcher
_dispatcher
;
QString
_infoString
;
QString
_warningString
;
QTimer
_timeoutTimer
;
QTimer
_connectionTimer
;
QNemoHeartbeat
_lastHeartbeat
;
FutureWatcher
_futureWatcher
;
};
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
static
StatusMap
statusMap
{
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
NOT_CONNECTED
,
"Not Connected"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
SYNC
,
"Synchronizing"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
READY
,
"Ready"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
TIMEOUT
,
"Timeout"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
INVALID_HEARTBEAT
,
"Error"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
WEBSOCKET_DETECTED
,
"Websocket Detected"
)};
...
...
@@ -150,32 +183,38 @@ NemoInterface::Impl::Impl(NemoInterface *p)
this
->
_pRosBridge
=
std
::
make_shared
<
RosbridgeWsClient
>
(
connectionString
.
toLocal8Bit
().
data
());
this
->
_pRosBridge
->
reset
();
qCritical
()
<<
"NemoInterface: add reset code here"
;
};
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
setConnectionString
();
// Heartbeat timeout.
connect
(
&
this
->
_timeoutTimer
,
&
QTimer
::
timeout
,
[
this
]
{
this
->
_setState
(
STATE
::
TIMEOUT
);
});
connect
(
&
this
->
_timeoutTimer
,
&
QTimer
::
timeout
,
[
this
]
{
this
->
_setState
(
STATE
::
HEARTBEAT_TIMEOUT
);
this
->
_doAction
();
});
// Connection timer (temporary workaround)
connect
(
&
this
->
_connectionTimer
,
&
QTimer
::
timeout
,
[
this
]
{
if
(
this
->
_pRosBridge
->
connected
())
{
if
(
this
->
_state
==
STATE
::
START_BRIDGE
||
this
->
_state
==
STATE
::
TIMEOUT
)
{
this
->
_state
==
STATE
::
WEBSOCKET_
TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_DETECTED
);
this
->
_doAction
();
}
}
else
{
if
(
this
->
_state
==
STATE
::
TRY_TOPIC_SERVICE_SETUP
||
this
->
_state
==
STATE
::
READY
)
{
this
->
_setState
(
STATE
::
TIMEOUT
);
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
WEBSOCKET_DETECTED
||
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_TIMEOUT
);
this
->
_doAction
();
}
}
});
connect
(
&
this
->
_futureWatcher
,
&
FutureWatcher
::
finished
,
[
this
]
{
this
->
_onFutureWatcherFinished
();
});
}
NemoInterface
::
Impl
::~
Impl
()
{}
...
...
@@ -190,91 +229,118 @@ void NemoInterface::Impl::start() {
void
NemoInterface
::
Impl
::
stop
()
{
if
(
running
())
{
this
->
_setState
(
STATE
::
STOPPED
);
this
->
_connectionTimer
.
stop
();
this
->
_doAction
();
}
}
std
::
future
<
QVariant
>
NemoInterface
::
Impl
::
addTiles
(
const
TileArray
&
tileArray
)
{
std
::
shared_
future
<
QVariant
>
NemoInterface
::
Impl
::
addTiles
(
const
Tile
Ptr
Array
&
tileArray
)
{
using
namespace
nemo_interface
;
if
(
this
->
ready
())
{
if
(
tileArray
.
size
()
>
0
)
{
// copy unknown tiles
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
for
(
const
auto
*
pTile
:
tileArray
)
{
auto
id
=
pTile
->
id
();
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
Q_ASSERT
(
it
==
_localTiles
.
end
());
if
(
Q_LIKELY
(
it
==
_localTiles
.
end
()))
{
auto
pTileCopy
=
std
::
make_shared
<
const
Tile
>
(
pTile
->
coordinateList
(),
0.0
,
id
);
_localTiles
.
insert
(
std
::
make_pair
(
id
,
pTileCopy
));
pTileArray
->
push_back
(
pTileCopy
);
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"addTiles(): tile with id: "
<<
pTile
->
id
()
<<
"already added."
;
}
}
// create command.
auto
pRosBridge
=
this
->
_pRosBridge
;
auto
pDispatcher
=
&
this
->
_dispatcher
;
Task
sendTilesCommand
([
pRosBridge
,
tileArray
,
pDispatcher
,
this
](
std
::
promise
<
QVariant
>
promise
)
{
// create json object
rapidjson
::
Document
request
;
auto
&
allocator
=
request
.
GetAllocator
();
rapidjson
::
Value
jsonTileArray
(
rapidjson
::
kArrayType
);
for
(
const
auto
&
tile
:
tileArray
)
{
const
auto
it
=
_tileMap
.
find
(
tile
.
id
());
if
(
Q_LIKELY
(
it
==
_tileMap
.
end
()))
{
using
namespace
ros_bridge
::
messages
;
rapidjson
::
Value
jsonTile
(
rapidjson
::
kObjectType
);
if
(
!
nemo_msgs
::
tile
::
toJson
(
tile
,
jsonTile
,
allocator
))
{
qCDebug
(
NemoInterfaceLog
)
<<
"addTiles(): not able to create json object: tile id: "
<<
tile
.
id
()
<<
" progress: "
<<
tile
.
progress
()
<<
" points: "
<<
tile
.
path
();
}
jsonTileArray
.
PushBack
(
jsonTile
,
allocator
);
}
}
// for
rapidjson
::
Value
tileKey
(
"in_tile_array"
);
request
.
AddMember
(
tileKey
,
jsonTileArray
,
allocator
);
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
void
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
mutable
{
qDebug
()
<<
"addTiles: in_message"
<<
in_message
->
string
().
c_str
();
promise_response
->
set_value
();
connection
->
send_close
(
1000
);
};
// call service.
pRosBridge
->
callService
(
"/nemo/add_tiles"
,
responseHandler
,
request
);
// wait for response.
auto
tStart
=
hrc
::
now
();
bool
abort
=
true
;
do
{
auto
status
=
future_response
.
wait_for
(
std
::
chrono
::
milliseconds
(
100
));
if
(
status
==
std
::
future_status
::
ready
)
{
abort
=
false
;
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
pDispatcher
->
interruptionPoint
());
// ready for send?
if
(
pTileArray
->
size
()
>
0
&&
(
this
->
ready
()
||
this
->
_userSync
()))
{
this
->
_setState
(
STATE
::
SYNC_USER
);
this
->
_doAction
();
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"Websocket not responding to request."
;
promise
.
set_value
(
QVariant
(
false
));
return
;
// create command.
auto
sendTilesCommand
=
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callAddTiles
,
this
,
pTileArray
));
// dispatch command and return.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
sendTilesCommand
));
auto
sfut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
sfut
);
return
sfut
;
}
}
std
::
promise
<
QVariant
>
p
;
p
.
set_value
(
QVariant
(
false
));
return
p
.
get_future
();
}
std
::
shared_future
<
QVariant
>
NemoInterface
::
Impl
::
removeTiles
(
const
IDArray
&
idArray
)
{
using
namespace
nemo_interface
;
if
(
idArray
.
size
()
>
0
)
{
// copy known ids
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
for
(
const
auto
id
:
idArray
)
{
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
Q_ASSERT
(
it
!=
_localTiles
.
end
());
if
(
Q_LIKELY
(
it
!=
_localTiles
.
end
()))
{
_localTiles
.
erase
(
it
);
pIdArray
->
push_back
(
id
);
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"removeTiles(): unknown id: "
<<
id
<<
"."
;
}
}
// ready for send?
if
(
pIdArray
->
size
()
>
0
&&
(
this
->
ready
()
||
this
->
_userSync
()))
{
this
->
_setState
(
STATE
::
SYNC_USER
);
this
->
_doAction
();
// create command.
auto
cmd
=
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callRemoveTiles
,
this
,
pIdArray
));
// dispatch command and return.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
cmd
));
auto
sfut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
sfut
);
return
sfut
;
}
}
std
::
promise
<
QVariant
>
p
;
p
.
set_value
(
QVariant
(
false
));
return
p
.
get_future
();
}
qCritical
()
<<
"addTiles(): ToDo: add return value checking here."
;
std
::
shared_future
<
QVariant
>
NemoInterface
::
Impl
::
clearTiles
()
{
using
namespace
nemo_interface
;
// update local tiles
QMetaObject
::
invokeMethod
(
this
->
_parent
,
std
::
bind
(
&
Impl
::
_addTilesLocal
,
this
,
tileArray
));
// clear local tiles (_localTiles)
this
->
_localTiles
.
clear
();
if
(
this
->
_localTiles
.
size
()
>
0
&&
(
this
->
ready
()
||
this
->
_userSync
()))
{
this
->
_setState
(
STATE
::
SYNC_USER
);
this
->
_doAction
();
// return success
promise
.
set_value
(
QVariant
(
true
));
return
;
});
// sendTilesCommand
// create command.
auto
pTask
=
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callClearTiles
,
this
));
// dispatch command and return.
auto
ret
=
_dispatcher
.
dispatch
(
sendTilesCommand
);
return
ret
;
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
sfut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
sfut
);
return
sfut
;
}
else
{
std
::
promise
<
QVariant
>
p
;
p
.
set_value
(
QVariant
(
false
));
...
...
@@ -286,9 +352,13 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
TileArray
tileArray
;
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_tileMap
.
find
(
id
);
if
(
it
!=
_tileMap
.
end
())
{
tileArray
.
append
(
it
->
second
);
const
auto
it
=
_localTiles
.
find
(
id
);
if
(
it
!=
_localTiles
.
end
())
{
MeasurementTile
copy
;
copy
.
setId
(
it
->
second
->
id
());
copy
.
setProgress
(
it
->
second
->
progress
());
copy
.
setPath
(
it
->
second
->
tile
());
tileArray
.
append
(
std
::
move
(
copy
));
}
}
...
...
@@ -298,8 +368,13 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
TileArray
NemoInterface
::
Impl
::
getAllTiles
()
{
TileArray
tileArray
;
for
(
const
auto
&
entry
:
_tileMap
)
{
tileArray
.
append
(
entry
.
second
);
for
(
const
auto
&
entry
:
_localTiles
)
{
auto
pTile
=
entry
.
second
;
MeasurementTile
copy
;
copy
.
setId
(
pTile
->
id
());
copy
.
setProgress
(
pTile
->
progress
());
copy
.
setPath
(
pTile
->
tile
());
tileArray
.
append
(
std
::
move
(
copy
));
}
return
tileArray
;
...
...
@@ -309,22 +384,30 @@ LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) {
LogicalArray
logicalArray
;
for
(
const
auto
&
id
:
idArray
)
{
const
auto
&
it
=
_
tileMap
.
find
(
id
);
logicalArray
.
append
(
it
!=
_
tileMap
.
end
());
const
auto
&
it
=
_
localTiles
.
find
(
id
);
logicalArray
.
append
(
it
!=
_
localTiles
.
end
());
}
return
logicalArray
;
}
std
::
size_t
NemoInterface
::
Impl
::
size
()
{
return
_
tileMap
.
size
();
}
std
::
size_t
NemoInterface
::
Impl
::
size
()
{
return
_
localTiles
.
size
();
}
bool
NemoInterface
::
Impl
::
empty
()
{
return
_
tileMap
.
empty
();
}
bool
NemoInterface
::
Impl
::
empty
()
{
return
_
localTiles
.
empty
();
}
ProgressArray
NemoInterface
::
Impl
::
getProgress
()
{
ProgressArray
progressArray
;
for
(
const
auto
&
entry
:
_tileMap
)
{
progressArray
.
append
(
TaggedProgress
{
entry
.
first
,
entry
.
second
.
progress
()});
if
(
this
->
_isSynchronized
())
{
for
(
const
auto
&
entry
:
_remoteTiles
)
{
progressArray
.
append
(
LabeledProgress
{
entry
.
second
->
progress
(),
entry
.
second
->
id
()});
}
}
else
{
for
(
const
auto
&
entry
:
_localTiles
)
{
progressArray
.
append
(
LabeledProgress
{
entry
.
second
->
progress
(),
entry
.
second
->
id
()});
}
}
return
progressArray
;
...
...
@@ -333,10 +416,21 @@ ProgressArray NemoInterface::Impl::getProgress() {
ProgressArray
NemoInterface
::
Impl
::
getProgress
(
const
IDArray
&
idArray
)
{
ProgressArray
progressArray
;
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_tileMap
.
find
(
id
);
if
(
it
!=
_tileMap
.
end
())
{
progressArray
.
append
(
TaggedProgress
{
it
->
first
,
it
->
second
.
progress
()});
if
(
this
->
_isSynchronized
())
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_remoteTiles
.
find
(
id
);
if
(
it
!=
_remoteTiles
.
end
())
{
progressArray
.
append
(
LabeledProgress
{
it
->
second
->
progress
(),
it
->
second
->
id
()});
}
}
}
else
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_localTiles
.
find
(
id
);
if
(
it
!=
_localTiles
.
end
())
{
progressArray
.
append
(
LabeledProgress
{
it
->
second
->
progress
(),
it
->
second
->
id
()});
}
}
}
...
...
@@ -344,93 +438,76 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
}
NemoInterface
::
STATUS
NemoInterface
::
Impl
::
status
()
{
NemoInterface
::
STATUS
status
;
_translate
(
this
->
_state
,
status
);
return
status
;
return
_status
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
running
()
{
return
_running
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
ready
()
{
return
_ready
(
this
->
_state
.
load
());
}
const
QString
&
NemoInterface
::
Impl
::
infoString
()
{
return
_infoString
;
}
const
QString
&
NemoInterface
::
Impl
::
warningString
()
{
return
_warningString
;
}
bool
NemoInterface
::
Impl
::
_sysSync
()
{
return
_sysSync
(
this
->
_state
);
}
void
NemoInterface
::
Impl
::
_addTilesLocal
(
const
TileArray
&
tileArray
)
{
bool
anyChanges
=
false
;
void
NemoInterface
::
Impl
::
_onFutureWatcherFinished
()
{
auto
lastTransactionSuccessfull
=
_futureWatcher
.
result
().
toBool
();
if
(
!
lastTransactionSuccessfull
)
{
if
(
this
->
_userSync
())
{
_trySynchronize
();
}
else
if
(
this
->
_sysSync
())
{
QTimer
::
singleShot
(
1000
,
[
this
]
{
this
->
_trySynchronize
();
});
}
}
else
{