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
7308bf55
Commit
7308bf55
authored
Jan 29, 2021
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
nemo interface improved
parent
29514d84
Changes
9
Hide whitespace changes
Inline
Side-by-side
Showing
9 changed files
with
753 additions
and
1108 deletions
+753
-1108
QGroundControl.AppImage
deploy/QGroundControl.AppImage
+0
-0
qgroundcontrol.pro
qgroundcontrol.pro
+0
-2
NemoInterface.cpp
src/MeasurementComplexItem/NemoInterface.cpp
+695
-672
TaskDispatcher.cpp
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp
+8
-3
TaskDispatcher.h
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h
+2
-3
rosbridge.cpp
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
+42
-1
rosbridge.h
src/MeasurementComplexItem/rosbridge/rosbridge.h
+6
-0
routing.cpp
src/MeasurementComplexItem/routing.cpp
+0
-423
routing.h
src/MeasurementComplexItem/routing.h
+0
-4
No files found.
deploy/QGroundControl.AppImage
View file @
7308bf55
No preview for this file type
qgroundcontrol.pro
View file @
7308bf55
...
...
@@ -463,7 +463,6 @@ HEADERS += \
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
tileHelper
.
h
\
src
/
MeasurementComplexItem
/
routing
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
h
\
src
/
QmlControls
/
QmlUnitsConversion
.
h
\
...
...
@@ -539,7 +538,6 @@ SOURCES += \
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
cpp
\
src
/
MeasurementComplexItem
/
routing
.
cpp
\
src
/
Vehicle
/
VehicleEscStatusFactGroup
.
cc
\
src
/
MeasurementComplexItem
/
AreaData
.
cc
\
src
/
api
/
QGCCorePlugin
.
cc
\
...
...
src/MeasurementComplexItem/NemoInterface.cpp
View file @
7308bf55
...
...
@@ -7,7 +7,7 @@
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <
shared_
mutex>
#include <mutex>
#include <QJsonArray>
#include <QJsonObject>
...
...
@@ -17,7 +17,6 @@
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "nemo_interface/FutureWatcher.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.h"
...
...
@@ -29,6 +28,30 @@
#include "ros_bridge/include/messages/nemo_msgs/tile_array.h"
#include "rosbridge/rosbridge.h"
/*
* Here some rules:
* * not threas safe functions are marked with *NotSafe(...)
* * If a function is not safe:
* * the call to it must be protected with a Lock.
* * not safe functions are not allowed to emit signals directly (danger of
* deadlock).
* * if a not safe function needs to emit a signal, defere it with
* QTimer::singleShot().
* * not safe functions are allowed to call other not safe functions
* * it is a bad idea to wait inside a not safe function for a asynchronous
* operation (potential deadlock)
* * Functions that are not marked with *NotSafe(...) must be thread safe!
*/
#define INVM(context, fun) \
{ \
auto value = QMetaObject::invokeMethod(context, fun); \
Q_ASSERT(value == true); \
Q_UNUSED(value); \
}
Q_DECLARE_METATYPE
(
ProgressArray
)
QGC_LOGGING_CATEGORY
(
NemoInterfaceLog
,
"NemoInterfaceLog"
)
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
...
...
@@ -36,10 +59,32 @@ QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define RESTART_RETRY_INTERVAl 2000 // ms
#define SYNC_INTERVAL 10000 // ms
#define SYNC_RETRY_INTERVAL 2000 // ms
static
constexpr
auto
maxResponseTime
=
std
::
chrono
::
milliseconds
(
10000
);
static
const
char
*
progressTopic
=
"/nemo/progress"
;
static
const
char
*
heartbeatTopic
=
"/nemo/heartbeat"
;
static
char
const
*
progressTopic
=
"/nemo/progress"
;
static
char
const
*
heartbeatTopic
=
"/nemo/heartbeat"
;
static
char
const
*
addTilesService
=
"/nemo/add_tiles"
;
static
char
const
*
removeTilesService
=
"/nemo/remove_tiles"
;
static
char
const
*
clearTilesService
=
"/nemo/clear_tiles"
;
static
char
const
*
getAllTilesService
=
"/nemo/get_all_tiles"
;
// static char const *getTilesService = "/nemo/get_tiles";
// static char const *containsTilesService = "/nemo/contains_tiles";
// static char const *extractTilesService = "/nemo/extract_tiles";
// static char const *sizeService = "/nemo/size";
// static char const *emptyService = "/nemo/empty";
static
char
const
*
getProgressService
=
"/nemo/get_progress"
;
static
char
const
*
getAllProgressService
=
"/nemo/get_all_progress"
;
static
char
const
*
getVersionService
=
"/nemo/get_version"
;
static
const
std
::
vector
<
char
const
*>
requiredServices
{
addTilesService
,
removeTilesService
,
clearTilesService
,
getAllTilesService
,
getAllProgressService
,
getProgressService
,
getVersionService
};
static
const
std
::
vector
<
char
const
*>
requiredTopics
{
progressTopic
,
heartbeatTopic
};
using
hrc
=
std
::
chrono
::
high_resolution_clock
;
using
ROSBridgePtr
=
std
::
shared_ptr
<
Rosbridge
>
;
...
...
@@ -51,8 +96,7 @@ typedef std::map<QString, std::shared_ptr<Tile>> TileMap;
typedef
std
::
map
<
QString
,
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
;
typedef
std
::
unique_lock
<
std
::
mutex
>
Lock
;
class
NemoInterface
::
Impl
{
enum
class
STATE
{
...
...
@@ -62,6 +106,7 @@ class NemoInterface::Impl {
TRY_SETUP
,
USER_SYNC
,
SYS_SYNC
,
SYNC_ERROR
,
READY
,
WEBSOCKET_TIMEOUT
,
HEARTBEAT_TIMEOUT
...
...
@@ -99,96 +144,84 @@ public:
const
QString
&
warningString
()
const
;
private:
void
_checkVersion
();
void
_subscribeProgressTopic
();
void
_subscribeHearbeatTopic
();
void
_doAction
();
void
_trySynchronize
();
void
_synchronizeIfNeccessary
();
void
_doSetup
();
void
_doActionNotSafe
();
void
_synchronize
();
void
_tryRestart
();
bool
_isSynchronizedNotSafe
()
const
;
bool
_isSynchronized
()
const
;
bool
_userSync
()
const
;
// thread safe
bool
_sysSync
()
const
;
// thread safe
void
_onFutureWatcherFinished
();
// thread safe
void
_onHeartbeatTimeout
();
// thread safe
void
_onHeartbeatTimeout
();
void
_onRosbridgeStateChanged
();
// 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
);
QVariant
_callGetAllProgress
();
QVariant
_callGetAllTiles
();
QVariant
_callGetVersion
();
enum
class
CALL_NAME
{
ADD_TILES
,
REMOVE_TILES
,
CLEAR_TILES
,
GET_PROGRESS
,
GET_ALL_TILES
,
GET_ALL_PROGRESS
,
GET_VERSION
};
QString
_toString
(
CALL_NAME
name
);
void
_addTilesRemote
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
,
std
::
promise
<
bool
>
promise
);
void
_addTilesRemote2
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
pTileArray
,
std
::
promise
<
bool
>
promise
);
void
_compareAndSync
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
pTileArray
,
std
::
promise
<
bool
>
promise
);
void
_setVersion
(
QString
version
,
std
::
promise
<
bool
>
promise
);
void
_removeTilesRemote
(
std
::
shared_ptr
<
IDArray
>
idArray
,
std
::
promise
<
bool
>
promise
);
void
_clearTilesRemote
(
std
::
promise
<
bool
>
promise
);
void
_updateProgress
(
std
::
shared_ptr
<
ProgressArray
>
pArray
,
std
::
promise
<
bool
>
promise
);
void
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
hb
,
std
::
promise
<
bool
>
promise
);
bool
_updateProgress
(
const
ProgressArray
&
pArray
);
void
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
hb
);
void
_setInfoStringNotSafe
(
const
QString
&
info
);
void
_setWarningStringNotSafe
(
const
QString
&
warning
);
void
_setInfoString
(
const
QString
&
info
);
void
_setWarningString
(
const
QString
&
warning
);
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
);
// state suff
bool
_userSync
()
const
;
bool
_sysSync
()
const
;
bool
_setStateNotSafe
(
STATE
newState
);
static
bool
_readyNotSafe
(
STATE
s
);
static
bool
_userSyncNotSafe
(
STATE
s
);
static
bool
_sysSyncNotSafe
(
STATE
s
);
static
bool
_runningNotSafe
(
STATE
s
);
static
NemoInterface
::
STATUS
_status
(
STATE
state
);
static
QString
_toString
(
STATE
s
);
static
QString
_toString
(
NemoInterface
::
STATUS
s
);
static
QString
_localVersion
;
QString
_remoteVersion
;
// impl functions
bool
_addTilesImpl
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
,
std
::
shared_ptr
<
const
IDArray
>
pIdArray
);
bool
_removeTilesImpl
(
std
::
shared_ptr
<
const
IDArray
>
pIdArray
);
bool
_clearTilesImpl
();
// Worker functions.
static
bool
_callAddTiles
(
const
QVector
<
std
::
shared_ptr
<
const
Tile
>>
&
tileArray
,
Dispatcher
&
d
,
Rosbridge
&
rb
);
static
bool
_callClearTiles
(
Dispatcher
&
d
,
Rosbridge
&
rb
);
static
ProgressArray
_callGetProgress
(
const
IDArray
&
pIdArray
,
Dispatcher
&
d
,
Rosbridge
&
rb
);
static
ProgressArray
_callGetAllProgress
(
Dispatcher
&
d
,
Rosbridge
&
rb
);
static
QVector
<
std
::
shared_ptr
<
Tile
>>
_callGetAllTiles
(
Dispatcher
&
d
,
Rosbridge
&
rb
);
static
QString
_callGetVersion
(
Dispatcher
&
d
,
Rosbridge
&
rb
);
static
bool
_callRemoveTiles
(
const
IDArray
&
pIdArray
,
Dispatcher
&
d
,
Rosbridge
&
rb
);
// functions to manipulate _remoteTiles
bool
_addTilesRemote
(
const
QVector
<
std
::
shared_ptr
<
const
Tile
>>
tileArray
);
bool
_addTilesRemote
(
const
QVector
<
std
::
shared_ptr
<
Tile
>>
&
tileArray
);
void
_removeTilesRemote
(
const
IDArray
&
idArray
);
void
_clearTilesRemote
();
void
_clearTilesRemoteNotSafe
();
// static and const members
static
const
char
*
_localVersion
;
NemoInterface
*
const
_parent
;
std
::
atomic
<
STATE
>
_state
;
std
::
atomic_bool
_versionOK
;
std
::
atomic_bool
_progressTopicOK
;
std
::
atomic_bool
_heartbeatTopicOK
;
std
::
atomic
<
CALL_NAME
>
_lastCall
;
// thread safe members
ROSBridgePtr
_pRosbridge
;
Dispatcher
_dispatcher
;
// protected by mutex
mutable
std
::
mutex
_m
;
STATE
_state
;
TileMap
_remoteTiles
;
TileMapConst
_localTiles
;
NemoInterface
*
const
_parent
;
Dispatcher
_dispatcher
;
QString
_infoString
;
QString
_warningString
;
// Members belonging to _parent->thread()
QTimer
_timeoutTimer
;
QTimer
_syncTimer
;
QTimer
_restartTimer
;
QNemoHeartbeat
_lastHeartbeat
;
FutureWatcher
_futureWatcher
;
};
QString
NemoInterface
::
Impl
::
_localVersion
(
"V_1.0"
);
const
char
*
NemoInterface
::
Impl
::
_localVersion
(
"V_1.0"
);
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
static
StatusMap
statusMap
{
...
...
@@ -206,8 +239,7 @@ static StatusMap statusMap{
NemoInterface
::
STATUS
::
WEBSOCKET_DETECTED
,
"Websocket Detected"
)};
NemoInterface
::
Impl
::
Impl
(
NemoInterface
*
p
)
:
_state
(
STATE
::
STOPPED
),
_versionOK
(
false
),
_progressTopicOK
(
false
),
_heartbeatTopicOK
(
false
),
_parent
(
p
)
{
:
_parent
(
p
),
_state
(
STATE
::
STOPPED
)
{
// ROS Bridge.
WimaSettings
*
wimaSettings
=
...
...
@@ -224,6 +256,7 @@ NemoInterface::Impl::Impl(NemoInterface *p)
this
->
start
();
}
};
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
setConnectionString
();
...
...
@@ -235,29 +268,31 @@ NemoInterface::Impl::Impl(NemoInterface *p)
connect
(
this
->
_pRosbridge
.
get
(),
&
Rosbridge
::
stateChanged
,
this
->
_parent
,
[
this
]
{
this
->
_onRosbridgeStateChanged
();
});
connect
(
&
this
->
_futureWatcher
,
&
FutureWatcher
::
finished
,
this
->
_parent
,
[
this
]
{
this
->
_onFutureWatcherFinished
();
});
connect
(
&
this
->
_restartTimer
,
&
QTimer
::
timeout
,
this
->
_parent
,
[
this
]
{
this
->
_tryRestart
();
});
connect
(
&
this
->
_syncTimer
,
&
QTimer
::
timeout
,
this
->
_parent
,
[
this
]
{
this
->
_synchronizeIfNeccessary
();
});
[
this
]
{
this
->
_synchronize
();
});
static
std
::
once_flag
flag
;
std
::
call_once
(
flag
,
[]
{
qRegisterMetaType
<
ProgressArray
>
();
});
}
NemoInterface
::
Impl
::~
Impl
()
{
this
->
_pRosbridge
->
stop
();
}
void
NemoInterface
::
Impl
::
start
()
{
if
(
!
running
())
{
this
->
_setState
(
STATE
::
START_BRIDGE
);
this
->
_doAction
();
Lock
lk
(
this
->
_m
);
if
(
!
_runningNotSafe
(
this
->
_state
))
{
this
->
_setStateNotSafe
(
STATE
::
START_BRIDGE
);
this
->
_doActionNotSafe
();
}
}
void
NemoInterface
::
Impl
::
stop
()
{
if
(
running
())
{
this
->
_setState
(
STATE
::
STOPPED
);
this
->
_doAction
();
Lock
lk
(
this
->
_m
);
if
(
_runningNotSafe
(
this
->
_state
))
{
this
->
_setStateNotSafe
(
STATE
::
STOPPED
);
this
->
_doActionNotSafe
();
}
}
...
...
@@ -272,6 +307,8 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
// copy unknown tiles
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
Lock
lk
(
this
->
_m
);
for
(
const
auto
*
pTile
:
tileArray
)
{
auto
id
=
pTile
->
id
();
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
...
...
@@ -288,38 +325,36 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
}
}
if
(
pTileArray
->
size
()
>
0
)
{
lk
.
unlock
();
emit
this
->
_parent
->
tilesChanged
();
lk
.
lock
();
}
// ready for send?
if
(
pTileArray
->
size
()
>
0
&&
(
this
->
ready
()
||
this
->
_userSync
()))
{
this
->
_setState
(
STATE
::
USER_SYNC
);
this
->
_doAction
();
// create add tiles command.
auto
pTask
=
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callAddTiles
,
this
,
pTileArray
));
// dispatch command.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
addFuture
=
ret
.
share
();
// create get progress cmd.
pTask
=
std
::
make_unique
<
Task
>
([
this
,
addFuture
,
pIdArray
]
{
addFuture
.
wait
();
if
(
addFuture
.
get
().
toBool
())
{
return
this
->
_callGetProgress
(
pIdArray
);
}
else
{
return
QVariant
(
false
);
if
(
pTileArray
->
size
()
>
0
&&
(
this
->
_readyNotSafe
(
this
->
_state
)
||
this
->
_userSyncNotSafe
(
this
->
_state
)))
{
this
->
_setStateNotSafe
(
STATE
::
USER_SYNC
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
// create task.
auto
pTask
=
std
::
make_unique
<
Task
>
([
this
,
pTileArray
,
pIdArray
]
{
auto
ret
=
this
->
_addTilesImpl
(
pTileArray
,
pIdArray
);
if
(
ret
)
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_isSynchronizedNotSafe
())
{
this
->
_setStateNotSafe
(
STATE
::
READY
);
this
->
_doActionNotSafe
();
}
}
return
ret
;
});
// dispatch command.
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
progressFuture
=
ret
.
share
();
_futureWatcher
.
setFuture
(
progressFuture
);
return
progressFuture
;
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
return
ret
.
share
();
}
}
...
...
@@ -338,6 +373,8 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
// copy known ids
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
Lock
lk
(
this
->
_m
);
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
Q_ASSERT
(
it
!=
_localTiles
.
end
());
...
...
@@ -349,23 +386,37 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
}
}
if
(
pIdArray
->
size
()
>
0
)
{
lk
.
unlock
();
emit
this
->
_parent
->
tilesChanged
();
lk
.
lock
();
}
// ready for send?
if
(
pIdArray
->
size
()
>
0
&&
(
this
->
ready
()
||
this
->
_userSync
()))
{
if
(
pIdArray
->
size
()
>
0
&&
(
this
->
_readyNotSafe
(
this
->
_state
)
||
this
->
_userSyncNotSafe
(
this
->
_state
)))
{
this
->
_setState
(
STATE
::
USER_SYNC
);
this
->
_doAction
();
this
->
_setStateNotSafe
(
STATE
::
USER_SYNC
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
// create command.
auto
cmd
=
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callRemoveTiles
,
this
,
pIdArray
));
auto
cmd
=
std
::
make_unique
<
Task
>
([
this
,
pIdArray
]
{
auto
ret
=
this
->
_removeTilesImpl
(
pIdArray
);
if
(
ret
)
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_isSynchronizedNotSafe
())
{
this
->
_setStateNotSafe
(
STATE
::
READY
);
this
->
_doActionNotSafe
();
}
}
return
ret
;
});
// dispatch command and return.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
cmd
));
auto
sfut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
sfut
);
return
sfut
;
}
}
...
...
@@ -381,26 +432,40 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
// qDebug() << "clearTiles called";
// clear local tiles (_localTiles)
Lock
lk
(
this
->
_m
);
if
(
!
_localTiles
.
empty
())
{
this
->
_localTiles
.
clear
();
lk
.
unlock
();
emit
this
->
_parent
->
tilesChanged
();
lk
.
lock
();
}
if
(
this
->
ready
()
||
this
->
_userSync
(
))
{
if
(
this
->
_readyNotSafe
(
this
->
_state
)
||
this
->
_readyNotSafe
(
this
->
_state
))
{
this
->
_setState
(
STATE
::
USER_SYNC
);
this
->
_doAction
();
this
->
_setStateNotSafe
(
STATE
::
USER_SYNC
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
// create command.
auto
pTask
=
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callClearTiles
,
this
));
auto
pTask
=
std
::
make_unique
<
Task
>
([
this
]
{
auto
ret
=
this
->
_clearTilesImpl
();
if
(
ret
)
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_isSynchronizedNotSafe
())
{
this
->
_setStateNotSafe
(
STATE
::
READY
);
this
->
_doActionNotSafe
();
}
}
return
QVariant
(
ret
);
});
// dispatch command and return.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
sfut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
sfut
);
return
sfut
;
return
ret
.
share
();
}
else
{
lk
.
unlock
();
std
::
promise
<
QVariant
>
p
;
p
.
set_value
(
QVariant
(
false
));
return
p
.
get_future
();
...
...
@@ -410,6 +475,7 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
TileArray
NemoInterface
::
Impl
::
getTiles
(
const
IDArray
&
idArray
)
const
{
TileArray
tileArray
;
Lock
lk
(
this
->
_m
);
if
(
this
->
ready
())
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_remoteTiles
.
find
(
id
);
...
...
@@ -440,6 +506,7 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) const {
TileArray
NemoInterface
::
Impl
::
getAllTiles
()
const
{
TileArray
tileArray
;
Lock
lk
(
this
->
_m
);
if
(
this
->
ready
())
{
for
(
const
auto
&
entry
:
_remoteTiles
)
{
auto
pTile
=
entry
.
second
;
...
...
@@ -467,6 +534,7 @@ TileArray NemoInterface::Impl::getAllTiles() const {
LogicalArray
NemoInterface
::
Impl
::
containsTiles
(
const
IDArray
&
idArray
)
const
{
LogicalArray
logicalArray
;
Lock
lk
(
this
->
_m
);
for
(
const
auto
&
id
:
idArray
)
{
const
auto
&
it
=
_localTiles
.
find
(
id
);
logicalArray
.
append
(
it
!=
_localTiles
.
end
());
...
...
@@ -475,14 +543,22 @@ LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) const {
return
logicalArray
;
}
std
::
size_t
NemoInterface
::
Impl
::
size
()
const
{
return
_localTiles
.
size
();
}
std
::
size_t
NemoInterface
::
Impl
::
size
()
const
{
Lock
lk
(
this
->
_m
);
return
_localTiles
.
size
();
}
bool
NemoInterface
::
Impl
::
empty
()
const
{
return
_localTiles
.
empty
();
}
bool
NemoInterface
::
Impl
::
empty
()
const
{
Lock
lk
(
this
->
_m
);
return
_localTiles
.
empty
();
}
ProgressArray
NemoInterface
::
Impl
::
getProgress
()
const
{
ProgressArray
progressArray
;
if
(
this
->
_isSynchronized
())
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_isSynchronizedNotSafe
())
{
for
(
const
auto
&
entry
:
_remoteTiles
)
{
progressArray
.
append
(
LabeledProgress
{
entry
.
second
->
progress
(),
entry
.
second
->
id
()});
...
...
@@ -500,7 +576,8 @@ ProgressArray NemoInterface::Impl::getProgress() const {
ProgressArray
NemoInterface
::
Impl
::
getProgress
(
const
IDArray
&
idArray
)
const
{
ProgressArray
progressArray
;
if
(
this
->
_isSynchronized
())
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_isSynchronizedNotSafe
())
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_remoteTiles
.
find
(
id
);
if
(
it
!=
_remoteTiles
.
end
())
{
...
...
@@ -522,173 +599,178 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) const {
}
NemoInterface
::
STATUS
NemoInterface
::
Impl
::
status
()
const
{
Lock
lk
(
this
->
_m
);
return
_status
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
running
()
const
{
return
_running
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
ready
()
const
{
return
_ready
(
this
->
_state
.
load
());
}
bool
NemoInterface
::
Impl
::
_sysSync
()
const
{
return
_sysSync
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
running
()
const
{
Lock
lk1
(
this
->
_m
);
return
_runningNotSafe
(
this
->
_state
);
}
void
NemoInterface
::
Impl
::
_onFutureWatcherFinished
()
{
if
(
this
->
ready
()
||
this
->
_userSync
()
||
this
->
_sysSync
())
{
static
long
tries
=
0
;
auto
lastTransactionSuccessfull
=
_futureWatcher
.
result
().
toBool
();
if
(
!
lastTransactionSuccessfull
)
{
++
tries
;
qCDebug
(
NemoInterfaceLog
)
<<
"last transaction unsuccessfull: "
<<
_toString
(
_lastCall
);
bool
NemoInterface
::
Impl
::
ready
()
const
{
Lock
lk1
(
this
->
_m
);
return
_readyNotSafe
(
this
->
_state
);
}
if
(
tries
<
5
)
{
QTimer
::
singleShot
(
SYNC_RETRY_INTERVAL
,
this
->
_parent
,
[
this
]
{
this
->
_trySynchronize
();
});
}
else
{
_setWarningString
(
"The last 5 transactions failed! Please check the "
"connection and consider reseting the connection."
);
tries
=
0
;
}
}
else
{
tries
=
0
;
}
}
bool
NemoInterface
::
Impl
::
_sysSync
()
const
{
Lock
lk1
(
this
->
_m
);
return
_sysSyncNotSafe
(
this
->
_state
);
}
void
NemoInterface
::
Impl
::
_onHeartbeatTimeout
()
{
this
->
_setState
(
STATE
::
HEARTBEAT_TIMEOUT
);
this
->
_doAction
();
Lock
lk1
(
this
->
_m
);
this
->
_setStateNotSafe
(
STATE
::
HEARTBEAT_TIMEOUT
);
this
->
_doActionNotSafe
();
}
void
NemoInterface
::
Impl
::
_onRosbridgeStateChanged
()
{
auto
state
=
this
->
_pRosbridge
->
state
();
if
(
state
==
Rosbridge
::
STATE
::
CONNECTED
)
{
auto
rbState
=
this
->
_pRosbridge
->
state
();
if
(
rbState
==
Rosbridge
::
STATE
::
CONNECTED
)
{
Lock
lk1
(
this
->
_m
);
if
(
this
->
_state
==
STATE
::
START_BRIDGE
||
this
->
_state
==
STATE
::
WEBSOCKET_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_DETECTED
);
this
->
_doAction
();
this
->
_setState
NotSafe
(
STATE
::
WEBSOCKET_DETECTED
);
this
->
_doAction
NotSafe
();
}
}
else
if
(
state
==
Rosbridge
::
STATE
::
TIMEOUT
)
{
}
else
if
(
rbState
==
Rosbridge
::
STATE
::
TIMEOUT
)
{
Lock
lk1
(
this
->
_m
);
if
(
this
->
_state
==
STATE
::
TRY_SETUP
||
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
WEBSOCKET_DETECTED
||
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_TIMEOUT
);
this
->
_doAction
();
this
->
_setState
NotSafe
(
STATE
::
WEBSOCKET_TIMEOUT
);
this
->
_doAction
NotSafe
();
}
}
}
bool
NemoInterface
::
Impl
::
_userSync
()
const
{
return
_userSync
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
_userSync
()
const
{
Lock
lk1
(
this
->
_m
);
return
_userSyncNotSafe
(
this
->
_state
);
}
const
QString
&
NemoInterface
::
Impl
::
infoString
()
const
{
return
_infoString
;
}
const
QString
&
NemoInterface
::
Impl
::
infoString
()
const
{
Lock
lk1
(
this
->
_m
);
return
_infoString
;
}
const
QString
&
NemoInterface
::
Impl
::
warningString
()
const
{
Lock
lk1
(
this
->
_m
);
return
_warningString
;
}
void
NemoInterface
::
Impl
::
_updateProgress
(
std
::
shared_ptr
<
ProgressArray
>
pArray
,
std
::
promise
<
bool
>
promise
)
{
// qDebug() << "_updateProgress called";
bool
NemoInterface
::
Impl
::
_updateProgress
(
const
ProgressArray
&
pArray
)
{
ProgressArray
copy
;
bool
error
=
false
;
for
(
auto
itLP
=
pArray
->
begin
();
itLP
!=
pArray
->
end
();)
{
Lock
lk1
(
this
->
_m
);
for
(
auto
itLP
=
pArray
.
begin
();
itLP
!=
pArray
.
end
();
++
itLP
)
{
auto
it
=
_remoteTiles
.
find
(
itLP
->
id
());
if
(
Q_LIKELY
(
it
!=
_remoteTiles
.
end
()))
{
it
->
second
->
setProgress
(
itLP
->
progress
());
++
itLP
;
copy
.
push_back
(
*
itLP
)
;
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"_updateProgress(): tile with id "
<<
itLP
->
id
()
<<
" not found."
;
itLP
=
pArray
->
erase
(
itLP
);
error
=
true
;
}
}
lk1
.
unlock
();
if
(
pArray
->
size
()
>
0
)
{
emit
_parent
->
progressChanged
(
*
pArra
y
);
if
(
copy
.
size
()
>
0
)
{
emit
_parent
->
progressChanged
(
cop
y
);
}
promise
.
set_value
(
!
error
)
;
return
!
error
;
}
void
NemoInterface
::
Impl
::
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
hb
,
std
::
promise
<
bool
>
promise
)
{
_lastHeartbeat
=
hb
;
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
if
(
this
->
_state
==
STATE
::
TRY_SETUP
)
{
this
->
_setState
(
STATE
::
READY
);
this
->
_doAction
();
}
else
if
(
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setState
(
STATE
::
READY
);
this
->
_doAction
();
void
NemoInterface
::
Impl
::
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
)
{
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
}))
Lock
lk
(
this
->
_m
);
if
(
this
->
_state
==
STATE
::
TRY_SETUP
||
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setStateNotSafe
(
STATE
::
READY
);
this
->
_doActionNotSafe
();
}
promise
.
set_value
(
true
);
}
void
NemoInterface
::
Impl
::
_setInfoString
(
const
QString
&
info
)
{
void
NemoInterface
::
Impl
::
_setInfoString
NotSafe
(
const
QString
&
info
)
{
if
(
_infoString
!=
info
)
{
_infoString
=
info
;
emit
this
->
_parent
->
infoStringChanged
();
// emit signal later, can't emit while mutex locked!
QTimer
::
singleShot
(
5
,
this
->
_parent
,
[
this
]
{
emit
this
->
_parent
->
infoStringChanged
();
});
}
}
void
NemoInterface
::
Impl
::
_setWarningString
(
const
QString
&
warning
)
{
void
NemoInterface
::
Impl
::
_setWarningString
NotSafe
(
const
QString
&
warning
)
{
if
(
_warningString
!=
warning
)
{
_warningString
=
warning
;
emit
this
->
_parent
->
warningStringChanged
();
// emit signal later, can't emit while mutex locked!
QTimer
::
singleShot
(
5
,
this
->
_parent
,
[
this
]
{
emit
this
->
_parent
->
warningStringChanged
();
});
}
}
void
NemoInterface
::
Impl
::
_checkVersion
()
{
void
NemoInterface
::
Impl
::
_setInfoString
(
const
QString
&
info
)
{
Lock
lk
(
this
->
_m
);
_setInfoStringNotSafe
(
info
);
}
void
NemoInterface
::
Impl
::
_setWarningString
(
const
QString
&
warning
)
{
Lock
lk
(
this
->
_m
);
_setWarningStringNotSafe
(
warning
);
}
void
NemoInterface
::
Impl
::
_doSetup
()
{
// create task
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
]
{
// wait for service
std
::
future
<
bool
>
fut
;
do
{
fut
=
this
->
_pRosbridge
->
serviceAvailable
(
"/nemo/get_version"
);
// wait
while
(
true
)
{
auto
status
=
fut
.
wait_for
(
std
::
chrono
::
milliseconds
(
5
));
if
(
this
->
_dispatcher
.
isInterruptionRequested
())
{
return
QVariant
(
false
);
}
if
(
status
==
std
::
future_status
::
ready
)
{
break
;
}
}
}
while
(
!
fut
.
get
());
auto
rb
=
this
->
_pRosbridge
;
auto
&
disp
=
this
->
_dispatcher
;
// call service
return
this
->
_callGetVersion
();
});
// check if required services are available
auto
cond
=
[
&
disp
]
{
return
!
disp
.
isInterruptionRequested
();
};
for
(
const
auto
&
cc
:
requiredServices
)
{
QString
s
(
cc
);
this
->
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
}
this
->
_setInfoString
(
"Waiting for required service "
+
s
);
void
NemoInterface
::
Impl
::
_subscribeProgressTopic
()
{
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
]
{
// wait for service
std
::
future
<
bool
>
fut
;
do
{
fut
=
this
->
_pRosbridge
->
topicAvailable
(
progressTopic
);
// wait
while
(
true
)
{
auto
status
=
fut
.
wait_for
(
std
::
chrono
::
milliseconds
(
5
));
if
(
this
->
_dispatcher
.
isInterruptionRequested
())
{
return
QVariant
(
false
);
}
if
(
status
==
std
::
future_status
::
ready
)
{
break
;
}
auto
available
=
rb
->
waitForService
(
cc
,
cond
);
if
(
!
available
)
{
return
QVariant
(
false
);
}
}
while
(
!
fut
.
get
());
}
// subscribe
this
->
_pRosbridge
->
subscribeTopic
(
progressTopic
,
[
this
](
const
QJsonObject
&
o
)
{
// check if version is compatible{
this
->
_setInfoString
(
"Checking version."
);
auto
version
=
_callGetVersion
(
disp
,
*
rb
);
if
(
version
!=
_localVersion
)
{
this
->
_setWarningString
(
"Version checking failed. Local protocol version ("
+
QString
(
_localVersion
)
+
") does not match remote version ("
+
version
+
")."
);
return
QVariant
(
false
);
}
// check if required topics are available
for
(
const
auto
&
cc
:
requiredTopics
)
{
QString
s
(
cc
);
this
->
_setInfoString
(
"Waiting for required topic "
+
s
);
auto
available
=
rb
->
waitForTopic
(
cc
,
cond
);
if
(
!
available
)
{
return
QVariant
(
false
);
}
}
// subscribe topics
rb
->
subscribeTopic
(
progressTopic
,
[
this
](
const
QJsonObject
&
o
)
{
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
progressArray
;
if
(
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
fromJson
(
...
...
@@ -707,167 +789,161 @@ void NemoInterface::Impl::_subscribeProgressTopic() {
}
if
(
rangeError
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress progress out "
qCWarning
(
NemoInterfaceLog
)
<<
progressTopic
<<
" progress out "
"of range, value was set to: "
<<
lp
.
progress
();
}
}
auto
p
=
std
::
make_shared
<
ProgressArray
>
();
*
p
=
std
::
move
(
progressArray
.
progress_array
());
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
p
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_updateProgress
(
p
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
future
.
wait
();
auto
ret
=
this
->
_updateProgress
(
progressArray
.
progress_array
());
if
(
!
ret
)
this
->
_setWarningString
(
"Progress update failed."
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress not able to "
qCWarning
(
NemoInterfaceLog
)
<<
progressTopic
<<
" not able to "
"create ProgressArray form json: "
<<
o
;
}
});
this
->
_progressTopicOK
=
true
;
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
/* context */
,
[
this
]()
{
this
->
_doAction
();
});
Q_ASSERT
(
value
==
true
);
return
QVariant
(
true
);
});
this
->
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
}
void
NemoInterface
::
Impl
::
_subscribeHearbeatTopic
()
{
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
]
{
// wait for service
std
::
future
<
bool
>
fut
;
do
{
fut
=
this
->
_pRosbridge
->
topicAvailable
(
heartbeatTopic
);
// wait
while
(
true
)
{
auto
status
=
fut
.
wait_for
(
std
::
chrono
::
milliseconds
(
5
));
if
(
this
->
_dispatcher
.
isInterruptionRequested
())
{
return
QVariant
(
false
);
}
if
(
status
==
std
::
future_status
::
ready
)
{
break
;
}
}
}
while
(
!
fut
.
get
());
// subscribe
using
namespace
ros_bridge
::
messages
;
this
->
_pRosbridge
->
subscribeTopic
(
heartbeatTopic
,
[
this
](
const
QJsonObject
&
o
)
{
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
o
,
heartbeat
))
{
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
heartbeat
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_onHeartbeatReceived
(
heartbeat
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
future
.
wait
();
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat not able to "
"create Heartbeat form json: "
<<
o
;
}
});
this
->
_heartbeatTopicOK
=
true
;
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
/* context */
,
[
this
]()
{
this
->
_doAction
();
});
Q_ASSERT
(
value
==
true
);
rb
->
subscribeTopic
(
heartbeatTopic
,
[
this
](
const
QJsonObject
&
o
)
{
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
o
,
heartbeat
))
{
this
->
_onHeartbeatReceived
(
heartbeat
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
heartbeatTopic
<<
" not able to "
"create Heartbeat form json: "
<<
o
;
}
});
// now ready
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
}))
_setInfoString
(
""
);
_setWarningString
(
""
);
return
QVariant
(
true
);
});
this
->
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
f
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
Q_UNUSED
(
f
);
}
void
NemoInterface
::
Impl
::
_trySynchronize
()
{
if
((
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
SYS_SYNC
||
this
->
_state
==
STATE
::
USER_SYNC
)
&&
!
_isSynchronized
())
{
void
NemoInterface
::
Impl
::
_synchronize
()
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
SYNC_ERROR
)
{
if
(
!
_dispatcher
.
idle
())
{
QTimer
::
singleShot
(
SYNC_RETRY_INTERVAL
,
this
->
_parent
,
[
this
]
{
this
->
_trySynchronize
();
});
qCWarning
(
NemoInterfaceLog
)
<<
"synchronization defered"
;
return
;
}
_setStateNotSafe
(
STATE
::
SYS_SYNC
);
_doActionNotSafe
();
qCWarning
(
NemoInterfaceLog
)
<<
"trying to synchronize"
;
// copy tiles.
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
for
(
auto
it
=
_localTiles
.
begin
();
it
!=
_localTiles
.
end
();
++
it
)
{
pTileArray
->
push_back
(
it
->
second
);
}
lk
.
unlock
();
// create cmd.
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
,
pTileArray
]
{
auto
rb
=
this
->
_pRosbridge
;
auto
&
disp
=
this
->
_dispatcher
;
// are _localTiles and _remoteTiles synced?
auto
remoteTiles
=
this
->
_callGetAllTiles
(
disp
,
*
rb
);
// create tile map;
std
::
map
<
QString
,
std
::
shared_ptr
<
Tile
>>
tempMap
;
for
(
auto
&&
pTile
:
remoteTiles
)
{
auto
it
=
tempMap
.
find
(
pTile
->
id
());
if
(
Q_LIKELY
(
it
==
tempMap
.
end
()))
{
tempMap
.
insert
(
std
::
make_pair
(
pTile
->
id
(),
pTile
));
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"_synchronizeIfNeccessary(): remoteTiles contains "
"duplicate id"
;
}
}
this
->
_setState
(
STATE
::
SYS_SYNC
);
this
->
_doAction
();
// compare with pTileArray
bool
synced
=
true
;
for
(
auto
&&
pTile
:
*
pTileArray
)
{
auto
it
=
tempMap
.
find
(
pTile
->
id
());
if
(
it
==
tempMap
.
end
()
||
it
->
second
->
tile
()
!=
pTile
->
tile
())
{
synced
=
false
;
break
;
}
}
// create clear cmd.
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
(
std
::
bind
(
&
Impl
::
_callClearTiles
,
this
));
if
(
!
synced
)
{
auto
success
=
this
->
_clearTilesImpl
();
if
(
!
success
)
{
Lock
lk
(
this
->
_m
);
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
_doActionNotSafe
();
return
QVariant
(
false
);
}
// dispatch command.
Q_ASSERT
(
_dispatcher
.
pendingTasks
()
==
0
);
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
clearFuture
=
ret
.
share
();
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
for
(
auto
&&
pTile
:
*
pTileArray
)
{
pIdArray
->
push_back
(
pTile
->
id
(
));
}
// create tile array.
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
for
(
const
auto
&
pair
:
_localTiles
)
{
pTileArray
->
push_back
(
pair
.
second
);
}
success
=
this
->
_addTilesImpl
(
pTileArray
,
pIdArray
);
// create addTiles cmd.
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
,
pTileArray
,
clearFuture
]
{
clearFuture
.
wait
();
if
(
clearFuture
.
get
().
toBool
())
{
return
this
->
_callAddTiles
(
pTileArray
);
}
else
{
return
QVariant
(
false
);
if
(
!
success
)
{
Lock
lk
(
this
->
_m
);
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
_doActionNotSafe
();
return
QVariant
(
false
);
}
}
else
{
// update progress
this
->
_clearTilesRemote
();
auto
ret
=
this
->
_addTilesRemote
(
*
pTileArray
);
if
(
ret
)
{
ProgressArray
progress
;
for
(
auto
&&
pTile
:
remoteTiles
)
{
progress
.
push_back
(
LabeledProgress
(
pTile
->
progress
(),
pTile
->
id
()));
}
});
ret
=
_updateProgress
(
progress
);
}
// dispatch command.
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
addFuture
=
ret
.
share
();
// create GetAllProgress cmd.
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
,
addFuture
]
{
addFuture
.
wait
();
if
(
addFuture
.
get
().
toBool
())
{
return
this
->
_callGetAllProgress
();
}
else
{
return
QVariant
(
false
);
if
(
!
ret
)
{
Lock
lk
(
this
->
_m
);
this
->
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
this
->
_setWarningString
(
"Getting progress failed."
);
qCDebug
(
NemoInterfaceLog
)
<<
"_addTilesImpl(): _updateProgress failed: unknown id."
;
return
QVariant
(
false
);
}
}
});
// dispatch command.
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
_futureWatcher
.
setFuture
(
ret
.
share
());
}
}
// check if local versions are still synced (user could habe modified
// _localTiles).
Lock
lk
(
this
->
_m
);
if
(
_isSynchronizedNotSafe
())
{
_setStateNotSafe
(
STATE
::
READY
);
_doActionNotSafe
();
}
else
{
INVM
(
this
->
_parent
,
[
this
]
{
this
->
_synchronize
();
})
}
lk
.
unlock
();
void
NemoInterface
::
Impl
::
_synchronizeIfNeccessary
()
{
if
(
_dispatcher
.
idle
())
{
// create getAllTiles cmd.
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
(
[
this
]
{
return
this
->
_callGetAllTiles
();
});
return
QVariant
(
true
);
});
// dispatch command.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
fut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
fut
);
_syncTimer
.
start
(
SYNC_INTERVAL
);
Q_UNUSED
(
ret
);
INVM
(
this
->
_parent
,
[
this
]
{
this
->
_syncTimer
.
start
(
SYNC_INTERVAL
);
})
}
else
{
_syncTimer
.
start
(
SYNC_RETRY_INTERVAL
);
INVM
(
this
->
_parent
,
[
this
]
{
this
->
_syncTimer
.
start
(
SYNC_RETRY_INTERVAL
);
})
}
}
...
...
@@ -884,88 +960,103 @@ void NemoInterface::Impl::_tryRestart() {
}
}
bool
NemoInterface
::
Impl
::
_isSynchronized
()
const
{
bool
NemoInterface
::
Impl
::
_isSynchronized
NotSafe
()
const
{
return
_localTiles
.
size
()
==
_remoteTiles
.
size
()
&&
std
::
equal
(
_localTiles
.
begin
(),
_localTiles
.
end
(),
_remoteTiles
.
begin
(),
[](
const
auto
&
a
,
const
auto
&
b
)
{
return
a
.
first
==
b
.
first
;
});
}
void
NemoInterface
::
Impl
::
_doAction
()
{
bool
NemoInterface
::
Impl
::
_isSynchronized
()
const
{
Lock
lk
(
this
->
_m
);
return
_isSynchronizedNotSafe
();
}
void
NemoInterface
::
Impl
::
_doActionNotSafe
()
{
static
bool
resetDone
=
false
;
switch
(
this
->
_state
)
{
case
STATE
:
:
STOPPED
:
_setWarningString
(
""
);
_setInfoString
(
""
);
this
->
_timeoutTimer
.
stop
();
this
->
_restartTimer
.
stop
();
this
->
_syncTimer
.
stop
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
case
STATE
:
:
STOPPED
:
{
std
::
promise
<
void
>
p
;
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_timeoutTimer
.
stop
();
this
->
_restartTimer
.
stop
();
this
->
_syncTimer
.
stop
();
}))
if
(
this
->
_pRosbridge
->
state
()
!=
Rosbridge
::
STATE
::
STOPPED
)
{
this
->
_pRosbridge
->
stop
();
}
_versionOK
=
false
;
_progressTopicOK
=
false
;
_heartbeatTopicOK
=
false
;
break
;
_dispatcher
.
stop
();
_dispatcher
.
clear
();
_setInfoStringNotSafe
(
""
);
_setWarningStringNotSafe
(
""
);
}
break
;
case
STATE
:
:
START_BRIDGE
:
{
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_restartTimer
.
start
(
RESTART_INTERVAl
);
}))
case
STATE
:
:
START_BRIDGE
:
this
->
_pRosbridge
->
start
();
this
->
_restartTimer
.
start
(
RESTART_INTERVAl
);
break
;
}
break
;
case
STATE
:
:
WEBSOCKET_DETECTED
:
resetDone
=
false
;
this
->
_setState
(
STATE
::
TRY_SETUP
);
this
->
_doAction
();
this
->
_setState
NotSafe
(
STATE
::
TRY_SETUP
);
this
->
_doAction
NotSafe
();
break
;
case
STATE
:
:
TRY_SETUP
:
if
(
!
_versionOK
)
{
this
->
_checkVersion
();
}
else
if
(
!
_progressTopicOK
)
{
this
->
_subscribeProgressTopic
();
}
else
if
(
!
_heartbeatTopicOK
)
{
this
->
_subscribeHearbeatTopic
();
}
else
{
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
}
break
;
case
STATE
:
:
READY
:
this
->
_trySynchronize
();
this
->
_syncTimer
.
start
(
SYNC_INTERVAL
);
this
->
_doSetup
();
break
;
case
STATE
:
:
READY
:
{
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_syncTimer
.
start
(
SYNC_INTERVAL
);
}))
if
(
!
_isSynchronizedNotSafe
())
{
// can't call this here directly.
QTimer
::
singleShot
(
100
,
this
->
_parent
,
[
this
]
{
this
->
_synchronize
();
});
}
_setInfoStringNotSafe
(
""
);
_setWarningStringNotSafe
(
""
);
}
break
;
case
STATE
:
:
USER_SYNC
:
case
STATE
:
:
SYS_SYNC
:
case
STATE
:
:
SYNC_ERROR
:
break
;
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
this
->
_syncTimer
.
stop
();
break
;
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
{
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_syncTimer
.
stop
();
}))
}
break
;
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
{
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_timeoutTimer
.
stop
();
this
->
_syncTimer
.
stop
();
}))
if
(
!
resetDone
)
{
resetDone
=
true
;
this
->
_pRosbridge
->
stop
();
this
->
_pRosbridge
->
start
();
}
this
->
_timeoutTimer
.
stop
();
this
->
_syncTimer
.
stop
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
_versionOK
=
false
;
_progressTopicOK
=
false
;
_heartbeatTopicOK
=
false
;
break
;
_dispatcher
.
stop
();
_dispatcher
.
clear
();
_setInfoStringNotSafe
(
""
);
_setWarningStringNotSafe
(
""
);
}
break
;
};
}
QVariant
NemoInterface
::
Impl
::
_callAddTiles
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
)
{
bool
NemoInterface
::
Impl
::
_callAddTiles
(
const
QVector
<
std
::
shared_ptr
<
const
Tile
>>
&
tileArray
,
Dispatcher
&
d
,
Rosbridge
&
rb
)
{
// qDebug() << "_callAddTiles called";
this
->
_lastCall
=
CALL_NAME
::
ADD_TILES
;
// create json object
QJsonArray
jsonTileArray
;
for
(
auto
&&
tile
:
*
pT
ileArray
)
{
for
(
auto
&&
tile
:
t
ileArray
)
{
using
namespace
ros_bridge
::
messages
;
QJsonObject
jsonTile
;
if
(
!
nemo_msgs
::
tile
::
toJson
(
*
tile
,
jsonTile
))
{
...
...
@@ -989,13 +1080,13 @@ QVariant NemoInterface::Impl::_callAddTiles(
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles
no
\"
success
\"
key or wrong type: "
<<
o
;
<<
addTilesService
<<
"
no
\"
success
\"
key or wrong type: "
<<
o
;
promise_response
->
set_value
(
false
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/add_tiles"
,
responseHandler
,
req
);
rb
.
callService
(
addTilesService
,
responseHandler
,
req
);
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1007,42 +1098,30 @@ QVariant NemoInterface::Impl::_callAddTiles(
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
d
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"addTiles(): Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
false
;
}
// transaction error?
if
(
!
future_response
.
get
())
{
return
QVariant
(
false
)
;
return
false
;
}
// add remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
/* context */
,
[
this
,
pTileArray
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_addTilesRemote
(
pTileArray
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
())
;
return
true
;
}
QVariant
NemoInterface
::
Impl
::
_callRemoveTiles
(
std
::
shared_ptr
<
IDArray
>
pIdArray
)
{
bool
NemoInterface
::
Impl
::
_callRemoveTiles
(
const
IDArray
&
pIdArray
,
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callRemoveTiles called";
this
->
_lastCall
=
CALL_NAME
::
REMOVE_TILES
;
// create json object
QJsonArray
jsonIdArray
;
for
(
auto
&&
id
:
*
pIdArray
)
{
for
(
auto
&&
id
:
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
append
(
id
);
}
// for
...
...
@@ -1059,13 +1138,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles
no
\"
success
\"
key or wrong type: "
<<
msg
;
<<
removeTilesService
<<
"
no
\"
success
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/remove_tiles"
,
responseHandler
,
req
);
rb
.
callService
(
removeTilesService
,
responseHandler
,
req
);
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1077,37 +1156,23 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
false
;
}
// transaction error?
if
(
!
future_response
.
get
())
{
return
QVariant
(
false
)
;
return
false
;
}
// remove remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
/* context */
,
[
this
,
pIdArray
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_removeTilesRemote
(
pIdArray
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
());
return
true
;
}
QVariant
NemoInterface
::
Impl
::
_callClearTiles
()
{
// qDebug() << "_callClearTiles called";
this
->
_lastCall
=
CALL_NAME
::
CLEAR_TILES
;
bool
NemoInterface
::
Impl
::
_callClearTiles
(
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
...
...
@@ -1118,8 +1183,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/clear_tiles"
,
responseHandler
,
QJsonObject
());
rb
.
callService
(
clearTilesService
,
responseHandler
,
QJsonObject
());
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1131,40 +1195,29 @@ QVariant NemoInterface::Impl::_callClearTiles() {
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
false
;
}
// transaction failed?
if
(
!
future_response
.
get
())
{
return
QVariant
(
false
)
;
return
false
;
}
// clear remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_clearTilesRemote
(
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
());
return
true
;
}
QVariant
NemoInterface
::
Impl
::
_callGetProgress
(
std
::
shared_ptr
<
IDArray
>
pIdArray
)
{
ProgressArray
NemoInterface
::
Impl
::
_callGetProgress
(
const
IDArray
&
pIdArray
,
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callGetProgress called";
this
->
_lastCall
=
CALL_NAME
::
GET_PROGRESS
;
// create json object
QJsonArray
jsonIdArray
;
for
(
auto
&&
id
:
*
pIdArray
)
{
for
(
auto
&&
id
:
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
append
(
id
);
}
// for
...
...
@@ -1185,15 +1238,15 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
promise_response
->
set_value
(
pArray
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_progress
error while creating ProgressArray "
"from json."
;
qCWarning
(
NemoInterfaceLog
)
<<
getProgressService
<<
"
error while creating ProgressArray "
"from json."
;
promise_response
->
set_value
(
nullptr
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_progress"
,
responseHandler
,
req
);
rb
.
callService
(
getProgressService
,
responseHandler
,
req
);
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1205,38 +1258,22 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
);
}
// transaction error?
auto
pArray
=
future_response
.
get
();
if
(
pArray
==
nullptr
)
{
return
QVariant
(
false
);
return
ProgressArray
();
}
// remove remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
pArray
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_updateProgress
(
pArray
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
()
);
return
*
future_response
.
get
(
);
}
QVariant
NemoInterface
::
Impl
::
_callGetAllProgress
()
{
ProgressArray
NemoInterface
::
Impl
::
_callGetAllProgress
(
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callGetAllProgress called";
this
->
_lastCall
=
CALL_NAME
::
GET_ALL_PROGRESS
;
// create response handler.
typedef
std
::
shared_ptr
<
ProgressArray
>
ResponseType
;
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
...
...
@@ -1251,17 +1288,16 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
promise_response
->
set_value
(
pArray
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_progress
error while creating ProgressArray "
"from json. msg: "
<<
o
;
qCWarning
(
NemoInterfaceLog
)
<<
getAllProgressService
<<
"
error while creating ProgressArray "
"from json. msg: "
<<
o
;
promise_response
->
set_value
(
nullptr
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_all_progress"
,
responseHandler
,
QJsonObject
());
rb
.
callService
(
getAllProgressService
,
responseHandler
,
QJsonObject
());
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1273,38 +1309,22 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"
all_remove_tile
s(): Websocket not responding to request."
;
return
QVariant
(
false
);
<<
"
_callGetAllProgres
s(): Websocket not responding to request."
;
return
ProgressArray
(
);
}
// transaction error?
auto
pArray
=
future_response
.
get
();
if
(
pArray
==
nullptr
)
{
return
QVariant
(
false
);
}
// remove remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
pArray
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_updateProgress
(
pArray
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
());
return
*
future_response
.
get
();
}
QVariant
NemoInterface
::
Impl
::
_callGetAllTiles
()
{
QVector
<
std
::
shared_ptr
<
Tile
>>
NemoInterface
::
Impl
::
_callGetAllTiles
(
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callGetAllProgress called";
this
->
_lastCall
=
CALL_NAME
::
GET_ALL_TILES
;
// create response handler.
typedef
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
ResponseType
;
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
...
...
@@ -1324,12 +1344,12 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
pArray
->
push_back
(
tile
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles
error while creating tile."
;
<<
getAllTilesService
<<
"
error while creating tile."
;
promise_response
->
set_value
(
nullptr
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles
json array does not contain objects."
;
<<
getAllTilesService
<<
"
json array does not contain objects."
;
promise_response
->
set_value
(
nullptr
);
}
}
...
...
@@ -1337,14 +1357,13 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
promise_response
->
set_value
(
pArray
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles
no tile_array key or wrong type."
;
<<
getAllTilesService
<<
"
no tile_array key or wrong type."
;
promise_response
->
set_value
(
nullptr
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_all_tiles"
,
responseHandler
,
QJsonObject
());
rb
.
callService
(
getAllTilesService
,
responseHandler
,
QJsonObject
());
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1356,35 +1375,18 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QV
ariant
(
false
);
return
QV
ector
<
std
::
shared_ptr
<
Tile
>>
(
);
}
// transaction error?
auto
pArray
=
future_response
.
get
();
if
(
pArray
==
nullptr
)
{
return
QVariant
(
false
);
}
// remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
pArray
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_compareAndSync
(
pArray
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
());
return
*
future_response
.
get
();
}
QVariant
NemoInterface
::
Impl
::
_callGetVersion
()
{
this
->
_lastCall
=
CALL_NAME
::
GET_VERSION
;
QString
NemoInterface
::
Impl
::
_callGetVersion
(
Dispatcher
&
d
,
Rosbridge
&
rb
)
{
// create response handler.
typedef
QString
ResponseType
;
...
...
@@ -1399,14 +1401,13 @@ QVariant NemoInterface::Impl::_callGetVersion() {
promise_response
->
set_value
(
version
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_version
no version key or wrong type."
;
<<
getVersionService
<<
"
no version key or wrong type."
;
promise_response
->
set_value
(
"error!"
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_version"
,
responseHandler
,
QJsonObject
());
rb
.
callService
(
getVersionService
,
responseHandler
,
QJsonObject
());
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1418,148 +1419,74 @@ QVariant NemoInterface::Impl::_callGetVersion() {
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
d
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
"unknown_version"
;
}
// transaction error?
auto
version
=
future_response
.
get
();
if
(
version
==
"error!"
)
{
return
QVariant
(
false
)
;
return
"unknown_version"
;
}
// remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
version
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_setVersion
(
version
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
());
}
QString
NemoInterface
::
Impl
::
_toString
(
NemoInterface
::
Impl
::
CALL_NAME
name
)
{
switch
(
name
)
{
case
CALL_NAME
:
:
ADD_TILES
:
return
QString
(
"ADD_TILES"
);
case
CALL_NAME
:
:
REMOVE_TILES
:
return
QString
(
"REMOVE_TILES"
);
case
CALL_NAME
:
:
CLEAR_TILES
:
return
QString
(
"CLEAR_TILES"
);
case
CALL_NAME
:
:
GET_PROGRESS
:
return
QString
(
"GET_PROGRESS"
);
case
CALL_NAME
:
:
GET_ALL_PROGRESS
:
return
QString
(
"GET_ALL_PROGRESS"
);
case
CALL_NAME
:
:
GET_ALL_TILES
:
return
QString
(
"GET_ALL_TILES"
);
case
CALL_NAME
:
:
GET_VERSION
:
return
QString
(
"GET_VERSION"
);
}
return
QString
(
"unknown CALL_NAME"
);
return
version
;
}
void
NemoInterface
::
Impl
::
_addTilesRemote
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
,
std
::
promise
<
bool
>
promise
)
{
bool
NemoInterface
::
Impl
::
_addTilesRemote
(
const
QVector
<
std
::
shared_ptr
<
const
Tile
>>
tileArray
)
{
// qDebug() << "_addTilesRemote called";
auto
pArrayDup
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
()
;
for
(
auto
&&
pTile
:
*
pT
ileArray
)
{
pArrayDup
->
push_back
(
std
::
make_shared
<
Tile
>
(
*
pTile
));
QVector
<
std
::
shared_ptr
<
Tile
>>
copy
;
for
(
auto
&&
pTile
:
t
ileArray
)
{
copy
.
push_back
(
std
::
make_shared
<
Tile
>
(
*
pTile
));
}
_addTilesRemote2
(
pArrayDup
,
std
::
move
(
promise
)
);
return
_addTilesRemote
(
copy
);
}
void
NemoInterface
::
Impl
::
_addTilesRemote2
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
pTileArray
,
std
::
promise
<
bool
>
promise
)
{
// qDebug() << "_addTilesRemote2 called";
bool
NemoInterface
::
Impl
::
_addTilesRemote
(
const
QVector
<
std
::
shared_ptr
<
Tile
>>
&
tileArray
)
{
bool
error
=
false
;
ProgressArray
array
;
bool
anyChange
=
false
;
for
(
auto
&&
pTile
:
*
pTileArray
)
{
Lock
lk
(
this
->
_m
);
for
(
auto
&&
pTile
:
tileArray
)
{
auto
id
=
pTile
->
id
();
auto
it
=
_remoteTiles
.
find
(
id
);
if
(
Q_LIKELY
(
it
==
_remoteTiles
.
end
()))
{
auto
ret
=
_remoteTiles
.
insert
(
std
::
make_pair
(
id
,
pTile
));
a
rray
.
push_back
(
LabeledProgress
(
pTile
->
progress
(),
pTile
->
id
()))
;
a
nyChange
=
true
;
Q_ASSERT
(
ret
.
second
==
true
);
Q_UNUSED
(
ret
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"_addTilesRemote: tile with id "
<<
id
<<
" already added."
;
if
(
pTile
->
tile
()
!=
it
->
second
->
tile
())
{
qCWarning
(
NemoInterfaceLog
)
<<
"_addTilesRemote: tiles differ but have the same id."
;
error
=
true
;
}
}
}
if
(
array
.
size
()
>
0
)
{
if
(
this
->
_isSynchronized
())
{
this
->
_setState
(
STATE
::
READY
);
this
->
_doAction
();
}
emit
this
->
_parent
->
progressChanged
(
array
);
}
promise
.
set_value
(
!
error
);
}
void
NemoInterface
::
Impl
::
_compareAndSync
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
pTileArray
,
std
::
promise
<
bool
>
promise
)
{
bool
synced
=
std
::
size_t
(
pTileArray
->
size
())
==
_localTiles
.
size
();
if
(
synced
)
{
for
(
auto
it
=
pTileArray
->
begin
();
it
!=
pTileArray
->
end
();
++
it
)
{
auto
match
=
_localTiles
.
find
((
*
it
)
->
id
());
if
(
match
==
_localTiles
.
end
())
{
synced
=
false
;
}
}
}
if
(
!
synced
)
{
qDebug
()
<<
"_compareAndSync: trying to synchronize"
;
_trySynchronize
();
}
else
{
_remoteTiles
.
clear
();
_addTilesRemote2
(
pTileArray
,
std
::
promise
<
bool
>
());
lk
.
unlock
();
if
(
anyChange
>
0
)
{
emit
this
->
_parent
->
tilesChanged
();
}
promise
.
set_value
(
true
);
}
void
NemoInterface
::
Impl
::
_setVersion
(
QString
version
,
std
::
promise
<
bool
>
promise
)
{
_remoteVersion
=
version
;
if
(
_remoteVersion
!=
_localVersion
)
{
_setWarningString
(
"Version checking failed. Local protocol version ("
+
_localVersion
+
") does not match remote version ("
+
_remoteVersion
+
")."
);
}
else
{
_versionOK
=
true
;
_doAction
();
}
promise
.
set_value
(
true
);
return
!
error
;
}
void
NemoInterface
::
Impl
::
_removeTilesRemote
(
std
::
shared_ptr
<
IDArray
>
idArray
,
std
::
promise
<
bool
>
promise
)
{
void
NemoInterface
::
Impl
::
_removeTilesRemote
(
const
IDArray
&
idArray
)
{
// qDebug() << "_removeTilesRemote called";
bool
anyChange
=
false
;
for
(
auto
&&
id
:
*
idArray
)
{
Lock
lk
(
this
->
_m
);
for
(
auto
&&
id
:
idArray
)
{
auto
it
=
_remoteTiles
.
find
(
id
);
if
(
Q_LIKELY
(
it
!=
_remoteTiles
.
end
()))
{
_remoteTiles
.
erase
(
it
);
...
...
@@ -1571,30 +1498,25 @@ void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr<IDArray> idArray,
}
if
(
anyChange
)
{
if
(
this
->
_isSynchronized
())
{
this
->
_setState
(
STATE
::
READY
);
this
->
_doAction
();
}
emit
this
->
_parent
->
tilesChanged
();
}
}
promise
.
set_value
(
true
);
void
NemoInterface
::
Impl
::
_clearTilesRemote
()
{
Lock
lk
(
this
->
_m
);
_clearTilesRemoteNotSafe
();
}
void
NemoInterface
::
Impl
::
_clearTilesRemote
(
std
::
promise
<
bool
>
promise
)
{
void
NemoInterface
::
Impl
::
_clearTilesRemote
NotSafe
(
)
{
// qDebug() << "_clearTilesRemote called";
if
(
_remoteTiles
.
size
()
>
0
)
{
_remoteTiles
.
clear
();
if
(
this
->
_isSynchronized
())
{
this
->
_setState
(
STATE
::
READY
);
this
->
_doAction
();
}
}
promise
.
set_value
(
true
);
}
bool
NemoInterface
::
Impl
::
_setState
(
STATE
newState
)
{
bool
NemoInterface
::
Impl
::
_setState
NotSafe
(
STATE
newState
)
{
if
(
newState
!=
this
->
_state
)
{
auto
oldState
=
this
->
_state
.
load
()
;
auto
oldState
=
this
->
_state
;
this
->
_state
=
newState
;
qCDebug
(
NemoInterfaceLog
)
...
...
@@ -1602,11 +1524,15 @@ bool NemoInterface::Impl::_setState(STATE newState) {
auto
oldStatus
=
_status
(
oldState
);
auto
newStatus
=
_status
(
newState
);
if
(
oldStatus
!=
newStatus
)
{
emit
this
->
_parent
->
statusChanged
();
// emit signal later
QTimer
::
singleShot
(
5
,
this
->
_parent
,
[
this
]
{
emit
this
->
_parent
->
statusChanged
();
});
}
if
(
_running
(
oldState
)
!=
_running
(
newState
))
{
emit
this
->
_parent
->
runningChanged
();
if
(
_runningNotSafe
(
oldState
)
!=
_runningNotSafe
(
newState
))
{
// emit signal later
QTimer
::
singleShot
(
5
,
this
->
_parent
,
[
this
]
{
emit
this
->
_parent
->
runningChanged
();
});
}
return
true
;
...
...
@@ -1615,19 +1541,19 @@ bool NemoInterface::Impl::_setState(STATE newState) {
}
}
bool
NemoInterface
::
Impl
::
_ready
(
NemoInterface
::
Impl
::
STATE
s
)
{
bool
NemoInterface
::
Impl
::
_ready
NotSafe
(
NemoInterface
::
Impl
::
STATE
s
)
{
return
s
==
STATE
::
READY
;
}
bool
NemoInterface
::
Impl
::
_userSync
(
NemoInterface
::
Impl
::
STATE
s
)
{
bool
NemoInterface
::
Impl
::
_userSync
NotSafe
(
NemoInterface
::
Impl
::
STATE
s
)
{
return
s
==
STATE
::
USER_SYNC
;
}
bool
NemoInterface
::
Impl
::
_sysSync
(
NemoInterface
::
Impl
::
STATE
s
)
{
bool
NemoInterface
::
Impl
::
_sysSync
NotSafe
(
NemoInterface
::
Impl
::
STATE
s
)
{
return
s
==
STATE
::
SYS_SYNC
;
}
bool
NemoInterface
::
Impl
::
_running
(
NemoInterface
::
Impl
::
STATE
s
)
{
bool
NemoInterface
::
Impl
::
_running
NotSafe
(
NemoInterface
::
Impl
::
STATE
s
)
{
return
s
!=
STATE
::
STOPPED
;
}
...
...
@@ -1654,6 +1580,9 @@ NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) {
case
STATE
:
:
SYS_SYNC
:
status
=
NemoInterface
::
STATUS
::
SYNC
;
break
;
case
STATE
:
:
SYNC_ERROR
:
status
=
NemoInterface
::
STATUS
::
ERROR
;
break
;
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
status
=
NemoInterface
::
STATUS
::
TIMEOUT
;
...
...
@@ -1672,13 +1601,15 @@ QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) {
case
STATE
:
:
WEBSOCKET_DETECTED
:
return
QString
(
"WEBSOCKET_DETECTED"
);
case
STATE
:
:
TRY_SETUP
:
return
QString
(
"TRY_
TOPIC_SERVICE_
SETUP"
);
return
QString
(
"TRY_SETUP"
);
case
STATE
:
:
READY
:
return
QString
(
"READY"
);
case
STATE
:
:
USER_SYNC
:
return
QString
(
"SYNC_USER"
);
case
STATE
:
:
SYS_SYNC
:
return
QString
(
"SYNC_SYS"
);
case
STATE
:
:
SYNC_ERROR
:
return
QString
(
"SYNC_ERROR"
);
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
return
QString
(
"WEBSOCKET_TIMEOUT"
);
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
...
...
@@ -1705,6 +1636,98 @@ QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) {
return
"unknown state!"
;
}
bool
NemoInterface
::
Impl
::
_addTilesImpl
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
,
std
::
shared_ptr
<
const
IDArray
>
pIdArray
)
{
auto
rb
=
this
->
_pRosbridge
;
auto
&
disp
=
this
->
_dispatcher
;
// add tiles
bool
success
=
this
->
_callAddTiles
(
*
pTileArray
,
disp
,
*
rb
);
if
(
!
success
)
{
this
->
_setWarningString
(
"Adding tiles failed. This might indicate a poor connection."
);
return
false
;
}
auto
ret
=
this
->
_addTilesRemote
(
*
pTileArray
);
if
(
!
ret
)
{
Lock
lk
(
this
->
_m
);
this
->
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
this
->
_setWarningString
(
"Adding tiles failed."
);
qCDebug
(
NemoInterfaceLog
)
<<
"_addTilesImpl(): _addTilesRemote return "
"false: different tiles with same id."
;
return
false
;
}
// fetch progress
auto
array
=
this
->
_callGetProgress
(
*
pIdArray
,
disp
,
*
rb
);
if
(
array
.
size
()
!=
pIdArray
->
size
())
{
Lock
lk
(
this
->
_m
);
this
->
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
this
->
_setWarningString
(
"Getting progress failed. This might "
"indicate a poor connection."
);
return
false
;
}
ret
=
this
->
_updateProgress
(
array
);
if
(
!
ret
)
{
Lock
lk
(
this
->
_m
);
this
->
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
this
->
_setWarningString
(
"Getting progress failed."
);
qCDebug
(
NemoInterfaceLog
)
<<
"_addTilesImpl(): _updateProgress failed: unknown id."
;
return
false
;
}
return
true
;
}
bool
NemoInterface
::
Impl
::
_removeTilesImpl
(
std
::
shared_ptr
<
const
IDArray
>
pIdArray
)
{
auto
rb
=
this
->
_pRosbridge
;
auto
&
disp
=
this
->
_dispatcher
;
auto
success
=
this
->
_callRemoveTiles
(
*
pIdArray
,
disp
,
*
rb
);
if
(
!
success
)
{
Lock
lk
(
this
->
_m
);
this
->
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
this
->
_setWarningString
(
"Removing tiles failed. This might "
"indicate a poor connection."
);
return
false
;
}
this
->
_removeTilesRemote
(
*
pIdArray
);
return
true
;
}
bool
NemoInterface
::
Impl
::
_clearTilesImpl
()
{
auto
rb
=
this
->
_pRosbridge
;
auto
&
disp
=
this
->
_dispatcher
;
auto
success
=
this
->
_callClearTiles
(
disp
,
*
rb
);
if
(
!
success
)
{
Lock
lk
(
this
->
_m
);
this
->
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
this
->
_doActionNotSafe
();
lk
.
unlock
();
this
->
_setWarningString
(
"Clear tiles failed. This might "
"indicate a poor connection."
);
return
false
;
}
this
->
_clearTilesRemote
();
return
true
;
}
// ===============================================================
// NemoInterface
NemoInterface
::
NemoInterface
()
...
...
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp
View file @
7308bf55
...
...
@@ -52,7 +52,11 @@ void TaskDispatcher::start() {
ULock
lk1
(
this
->
_mutex
);
if
(
!
_running
)
{
this
->
_running
=
true
;
_future
=
QtConcurrent
::
run
([
this
]()
mutable
{
return
this
->
run
();
});
auto
p
=
std
::
make_shared
<
std
::
promise
<
void
>>
();
_threadFuture
=
p
->
get_future
();
QtConcurrent
::
run
([
this
,
p
=
std
::
move
(
p
)]()
mutable
{
return
this
->
run
(
std
::
move
(
*
p
));
});
lk1
.
unlock
();
}
}
...
...
@@ -61,8 +65,8 @@ void TaskDispatcher::stop() {
ULock
lk1
(
this
->
_mutex
);
if
(
_running
)
{
this
->
_running
=
false
;
_threadFuture
.
wait
();
lk1
.
unlock
();
this
->
_future
.
waitForFinished
();
}
}
...
...
@@ -88,7 +92,7 @@ std::size_t TaskDispatcher::pendingTasks() {
bool
TaskDispatcher
::
idle
()
{
return
this
->
pendingTasks
()
==
0
;
}
void
TaskDispatcher
::
run
()
{
void
TaskDispatcher
::
run
(
std
::
promise
<
void
>
p
)
{
while
(
true
)
{
ULock
lk1
(
this
->
_mutex
);
...
...
@@ -112,6 +116,7 @@ void TaskDispatcher::run() {
break
;
}
}
p
.
set_value
();
}
}
// namespace nemo_interface
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h
View file @
7308bf55
...
...
@@ -56,13 +56,12 @@ public:
bool
idle
();
protected:
void
run
();
void
run
(
std
::
promise
<
void
>
p
);
private:
QFuture
<
void
>
_future
;
std
::
deque
<
std
::
unique_ptr
<
Task
>>
_taskQueue
;
std
::
deque
<
std
::
promise
<
QVariant
>>
_promiseQueue
;
std
::
future
<
void
>
_threadFuture
;
bool
_running
;
std
::
mutex
_mutex
;
};
...
...
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
View file @
7308bf55
...
...
@@ -37,7 +37,7 @@ void Rosbridge::start() {
_impl
->
moveToThread
(
_t
);
connect
(
_impl
,
&
RosbridgeImpl
::
stateChanged
,
this
,
&
Rosbridge
::
s
tateChanged
);
&
Rosbridge
::
_onImplS
tateChanged
);
connect
(
this
,
&
Rosbridge
::
_start
,
_impl
,
&
RosbridgeImpl
::
start
);
connect
(
this
,
&
Rosbridge
::
_stop
,
_impl
,
&
RosbridgeImpl
::
stop
);
...
...
@@ -173,6 +173,26 @@ void Rosbridge::waitForTopic(const QString &topic) {
}
}
bool
Rosbridge
::
waitForTopic
(
const
QString
&
topic
,
const
std
::
function
<
bool
()
>
&
condition
)
{
std
::
future
<
bool
>
fut
;
do
{
fut
=
topicAvailable
(
topic
);
// wait
while
(
true
)
{
auto
status
=
fut
.
wait_for
(
std
::
chrono
::
milliseconds
(
5
));
if
(
!
condition
())
{
return
false
;
}
if
(
status
==
std
::
future_status
::
ready
)
{
break
;
}
}
}
while
(
!
fut
.
get
());
return
true
;
}
void
Rosbridge
::
advertiseService
(
const
QString
&
service
,
const
QString
&
type
,
CallBackWReturn
callback
)
{
emit
_advertiseService
(
service
,
type
,
callback
);
...
...
@@ -244,6 +264,26 @@ void Rosbridge::waitForService(const QString &service) {
}
}
bool
Rosbridge
::
waitForService
(
const
QString
&
service
,
const
std
::
function
<
bool
()
>
&
condition
)
{
std
::
future
<
bool
>
fut
;
do
{
fut
=
serviceAvailable
(
service
);
// wait
while
(
true
)
{
auto
status
=
fut
.
wait_for
(
std
::
chrono
::
milliseconds
(
5
));
if
(
!
condition
())
{
return
false
;
}
if
(
status
==
std
::
future_status
::
ready
)
{
break
;
}
}
}
while
(
!
fut
.
get
());
return
true
;
}
Rosbridge
::
STATE
translate
(
RosbridgeImpl
::
STATE
s
)
{
switch
(
s
)
{
case
RosbridgeImpl
:
:
STATE
::
STOPPED
:
...
...
@@ -260,3 +300,4 @@ Rosbridge::STATE translate(RosbridgeImpl::STATE s) {
qCritical
()
<<
"unreachable branch!"
;
return
Rosbridge
::
STATE
::
STOPPED
;
}
void
Rosbridge
::
_onImplStateChanged
()
{
emit
stateChanged
();
}
src/MeasurementComplexItem/rosbridge/rosbridge.h
View file @
7308bf55
...
...
@@ -58,6 +58,8 @@ public:
void
unsubscribeAllTopics
();
void
waitForTopic
(
const
QString
&
topic
);
bool
waitForTopic
(
const
QString
&
topic
,
const
std
::
function
<
bool
()
>
&
condition
);
// Services.
void
advertiseService
(
const
QString
&
service
,
const
QString
&
type
,
...
...
@@ -75,6 +77,8 @@ public:
std
::
future
<
QJsonObject
>
getAdvertisedServices
();
void
waitForService
(
const
QString
&
service
);
bool
waitForService
(
const
QString
&
service
,
const
std
::
function
<
bool
()
>
&
condition
);
signals:
void
stateChanged
();
...
...
@@ -99,6 +103,8 @@ signals:
const
QJsonObject
&
args
=
QJsonObject
());
void
_unadvertiseService
(
const
QString
&
service
);
void
_unadvertiseAllServices
();
private
slots
:
void
_onImplStateChanged
();
private:
std
::
atomic
<
STATE
>
_state
;
...
...
src/MeasurementComplexItem/routing.cpp
deleted
100644 → 0
View file @
29514d84
#include "routing.h"
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
using
namespace
operations_research
;
// Aux struct and functions.
struct
InternalParameters
{
InternalParameters
()
:
numSolutionsPerRun
(
1
),
numRuns
(
1
),
minNumTransectsPerRun
(
5
),
stop
([]
{
return
false
;
})
{}
std
::
size_t
numSolutionsPerRun
;
std
::
size_t
numRuns
;
std
::
size_t
minNumTransectsPerRun
;
std
::
function
<
bool
(
void
)
>
stop
;
mutable
std
::
string
errorString
;
};
bool
getRoute
(
const
FPolygon
&
area
,
const
LineStringArray
&
transects
,
std
::
vector
<
Solution
>
&
solutionVector
,
const
InternalParameters
&
par
=
InternalParameters
());
bool
getRoute
(
const
FPolygon
&
area
,
const
LineStringArray
&
transects
,
std
::
vector
<
Solution
>
&
solutionVector
,
const
InternalParameters
&
par
)
{
#ifdef SNAKE_SHOW_TIME
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
//================================================================
// Create routing model.
//================================================================
// Use integer polygons to increase numerical robustness.
// Convert area;
IPolygon
intArea
;
for
(
const
auto
&
v
:
area
.
outer
())
{
auto
p
=
float2Int
(
v
);
intArea
.
outer
().
push_back
(
p
);
}
for
(
const
auto
&
ring
:
area
.
inners
())
{
IRing
intRing
;
for
(
const
auto
&
v
:
ring
)
{
auto
p
=
float2Int
(
v
);
intRing
.
push_back
(
p
);
}
intArea
.
inners
().
push_back
(
std
::
move
(
intRing
));
}
// Helper classes.
struct
VirtualNode
{
VirtualNode
(
std
::
size_t
f
,
std
::
size_t
t
)
:
fromIndex
(
f
),
toIndex
(
t
)
{}
std
::
size_t
fromIndex
;
// index for leaving node
std
::
size_t
toIndex
;
// index for entering node
};
struct
NodeToTransect
{
NodeToTransect
(
std
::
size_t
i
,
bool
r
)
:
transectsIndex
(
i
),
reversed
(
r
)
{}
std
::
size_t
transectsIndex
;
// transects index
bool
reversed
;
// transect reversed?
};
// Create vertex and node list
std
::
vector
<
IPoint
>
vertices
;
std
::
vector
<
std
::
pair
<
std
::
size_t
,
std
::
size_t
>>
disjointNodes
;
std
::
vector
<
VirtualNode
>
nodeList
;
std
::
vector
<
NodeToTransect
>
nodeToTransectList
;
for
(
std
::
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
)
{
const
auto
&
t
=
transects
[
i
];
// Copy line edges only.
if
(
t
.
size
()
==
1
||
i
==
0
)
{
auto
p
=
float2Int
(
t
.
back
());
vertices
.
push_back
(
p
);
nodeToTransectList
.
emplace_back
(
i
,
false
);
auto
idx
=
vertices
.
size
()
-
1
;
nodeList
.
emplace_back
(
idx
,
idx
);
}
else
if
(
t
.
size
()
>
1
)
{
auto
p1
=
float2Int
(
t
.
front
());
auto
p2
=
float2Int
(
t
.
back
());
vertices
.
push_back
(
p1
);
vertices
.
push_back
(
p2
);
nodeToTransectList
.
emplace_back
(
i
,
false
);
nodeToTransectList
.
emplace_back
(
i
,
true
);
auto
fromIdx
=
vertices
.
size
()
-
1
;
auto
toIdx
=
fromIdx
-
1
;
nodeList
.
emplace_back
(
fromIdx
,
toIdx
);
nodeList
.
emplace_back
(
toIdx
,
fromIdx
);
disjointNodes
.
emplace_back
(
toIdx
,
fromIdx
);
}
else
{
// transect empty
std
::
cout
<<
"ignoring empty transect with index "
<<
i
<<
std
::
endl
;
}
}
#ifdef SNAKE_DEBUG
// Print.
std
::
cout
<<
"nodeToTransectList:"
<<
std
::
endl
;
std
::
cout
<<
"node:transectIndex:reversed"
<<
std
::
endl
;
std
::
size_t
c
=
0
;
for
(
const
auto
&
n2t
:
nodeToTransectList
)
{
std
::
cout
<<
c
++
<<
":"
<<
n2t
.
transectsIndex
<<
":"
<<
n2t
.
reversed
<<
std
::
endl
;
}
std
::
cout
<<
"nodeList:"
<<
std
::
endl
;
std
::
cout
<<
"node:fromIndex:toIndex"
<<
std
::
endl
;
c
=
0
;
for
(
const
auto
&
n
:
nodeList
)
{
std
::
cout
<<
c
++
<<
":"
<<
n
.
fromIndex
<<
":"
<<
n
.
toIndex
<<
std
::
endl
;
}
std
::
cout
<<
"disjoint nodes:"
<<
std
::
endl
;
std
::
cout
<<
"number:nodes"
<<
std
::
endl
;
c
=
0
;
for
(
const
auto
&
d
:
disjointNodes
)
{
std
::
cout
<<
c
++
<<
":"
<<
d
.
first
<<
","
<<
d
.
second
<<
std
::
endl
;
}
#endif
// Add polygon vertices.
for
(
auto
&
v
:
intArea
.
outer
())
{
vertices
.
push_back
(
v
);
}
for
(
auto
&
ring
:
intArea
.
inners
())
{
for
(
auto
&
v
:
ring
)
{
vertices
.
push_back
(
v
);
}
}
// Create connection graph (inf == no connection between vertices).
// Note: graph is not symmetric.
auto
n
=
vertices
.
size
();
// Matrix must be double since integers don't have infinity and nan
Matrix
<
double
>
connectionGraph
(
n
,
n
);
for
(
std
::
size_t
i
=
0
;
i
<
n
;
++
i
)
{
auto
&
fromVertex
=
vertices
[
i
];
for
(
std
::
size_t
j
=
0
;
j
<
n
;
++
j
)
{
auto
&
toVertex
=
vertices
[
j
];
ILineString
line
{
fromVertex
,
toVertex
};
if
(
bg
::
covered_by
(
line
,
intArea
))
{
connectionGraph
(
i
,
j
)
=
bg
::
length
(
line
);
}
else
{
connectionGraph
(
i
,
j
)
=
std
::
numeric_limits
<
double
>::
infinity
();
}
}
}
#ifdef SNAKE_DEBUG
std
::
cout
<<
"connection grah:"
<<
std
::
endl
;
std
::
cout
<<
connectionGraph
<<
std
::
endl
;
#endif
// Create distance matrix.
auto
distLambda
=
[
&
connectionGraph
](
std
::
size_t
i
,
std
::
size_t
j
)
->
double
{
return
connectionGraph
(
i
,
j
);
};
auto
nNodes
=
nodeList
.
size
();
Matrix
<
IntType
>
distanceMatrix
(
nNodes
,
nNodes
);
for
(
std
::
size_t
i
=
0
;
i
<
nNodes
;
++
i
)
{
distanceMatrix
(
i
,
i
)
=
0
;
for
(
std
::
size_t
j
=
i
+
1
;
j
<
nNodes
;
++
j
)
{
auto
dist
=
connectionGraph
(
i
,
j
);
if
(
std
::
isinf
(
dist
))
{
std
::
vector
<
std
::
size_t
>
route
;
if
(
!
dijkstraAlgorithm
(
n
,
i
,
j
,
route
,
dist
,
distLambda
))
{
std
::
stringstream
ss
;
ss
<<
"Distance matrix calculation failed. connection graph: "
<<
connectionGraph
<<
std
::
endl
;
ss
<<
"area: "
<<
bg
::
wkt
(
area
)
<<
std
::
endl
;
ss
<<
"transects:"
<<
std
::
endl
;
for
(
const
auto
&
t
:
transects
)
{
ss
<<
bg
::
wkt
(
t
)
<<
std
::
endl
;
}
par
.
errorString
=
ss
.
str
();
return
false
;
}
(
void
)
route
;
}
distanceMatrix
(
i
,
j
)
=
dist
;
distanceMatrix
(
j
,
i
)
=
dist
;
}
}
#ifdef SNAKE_DEBUG
std
::
cout
<<
"distance matrix:"
<<
std
::
endl
;
std
::
cout
<<
distanceMatrix
<<
std
::
endl
;
#endif
// Create (asymmetric) routing matrix.
Matrix
<
IntType
>
routingMatrix
(
nNodes
,
nNodes
);
for
(
std
::
size_t
i
=
0
;
i
<
nNodes
;
++
i
)
{
auto
fromNode
=
nodeList
[
i
];
for
(
std
::
size_t
j
=
0
;
j
<
nNodes
;
++
j
)
{
auto
toNode
=
nodeList
[
j
];
routingMatrix
(
i
,
j
)
=
distanceMatrix
(
fromNode
.
fromIndex
,
toNode
.
toIndex
);
}
}
// Insert max for disjoint nodes.
for
(
const
auto
&
d
:
disjointNodes
)
{
auto
i
=
d
.
first
;
auto
j
=
d
.
second
;
routingMatrix
(
i
,
j
)
=
std
::
numeric_limits
<
IntType
>::
max
();
routingMatrix
(
j
,
i
)
=
std
::
numeric_limits
<
IntType
>::
max
();
}
#ifdef SNAKE_DEBUG
std
::
cout
<<
"routing matrix:"
<<
std
::
endl
;
std
::
cout
<<
routingMatrix
<<
std
::
endl
;
#endif
// Create Routing Index Manager.
auto
minNumTransectsPerRun
=
std
::
max
<
std
::
size_t
>
(
1
,
par
.
minNumTransectsPerRun
);
auto
maxRuns
=
std
::
max
<
std
::
size_t
>
(
1
,
std
::
floor
(
double
(
transects
.
size
())
/
minNumTransectsPerRun
));
auto
numRuns
=
std
::
max
<
std
::
size_t
>
(
1
,
par
.
numRuns
);
numRuns
=
std
::
min
<
std
::
size_t
>
(
numRuns
,
maxRuns
);
RoutingIndexManager
::
NodeIndex
depot
(
0
);
// std::vector<RoutingIndexManager::NodeIndex> depots(numRuns, depot);
// RoutingIndexManager manager(nNodes, numRuns, depots, depots);
RoutingIndexManager
manager
(
nNodes
,
numRuns
,
depot
);
// Create Routing Model.
RoutingModel
routing
(
manager
);
// Create and register a transit callback.
const
int
transitCallbackIndex
=
routing
.
RegisterTransitCallback
(
[
&
routingMatrix
,
&
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
routingMatrix
(
from_node
,
to_node
);
});
// Define cost of each arc.
routing
.
SetArcCostEvaluatorOfAllVehicles
(
transitCallbackIndex
);
// Add distance dimension.
if
(
numRuns
>
1
)
{
routing
.
AddDimension
(
transitCallbackIndex
,
0
,
300000000
,
true
,
// start cumul to zero
"Distance"
);
routing
.
GetMutableDimension
(
"Distance"
)
->
SetGlobalSpanCostCoefficient
(
100000000
);
}
// Define disjunctions.
#ifdef SNAKE_DEBUG
std
::
cout
<<
"disjunctions:"
<<
std
::
endl
;
#endif
for
(
const
auto
&
d
:
disjointNodes
)
{
auto
i
=
d
.
first
;
auto
j
=
d
.
second
;
#ifdef SNAKE_DEBUG
std
::
cout
<<
i
<<
","
<<
j
<<
std
::
endl
;
#endif
auto
idx0
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
i
));
auto
idx1
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
j
));
std
::
vector
<
int64
>
disj
{
idx0
,
idx1
};
routing
.
AddDisjunction
(
disj
,
-
1
/*force cardinality*/
,
1
/*cardinality*/
);
}
// Set first solution heuristic.
auto
searchParameters
=
DefaultRoutingSearchParameters
();
searchParameters
.
set_first_solution_strategy
(
FirstSolutionStrategy
::
PATH_CHEAPEST_ARC
);
// Number of solutions.
auto
numSolutionsPerRun
=
std
::
max
<
std
::
size_t
>
(
1
,
par
.
numSolutionsPerRun
);
searchParameters
.
set_number_of_solutions_to_collect
(
numSolutionsPerRun
);
// Set costume limit.
auto
*
solver
=
routing
.
solver
();
auto
*
limit
=
solver
->
MakeCustomLimit
(
par
.
stop
);
routing
.
AddSearchMonitor
(
limit
);
#ifdef SNAKE_SHOW_TIME
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"create routing model: "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
//================================================================
// Solve model.
//================================================================
#ifdef SNAKE_SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
auto
pSolutions
=
std
::
make_unique
<
std
::
vector
<
const
Assignment
*>>
();
(
void
)
routing
.
SolveWithParameters
(
searchParameters
,
pSolutions
.
get
());
#ifdef SNAKE_SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"solve routing model: "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
if
(
par
.
stop
())
{
par
.
errorString
=
"User terminated."
;
return
false
;
}
if
(
pSolutions
->
size
()
==
0
)
{
std
::
stringstream
ss
;
ss
<<
"No solution found."
<<
std
::
endl
;
par
.
errorString
=
ss
.
str
();
return
false
;
}
//================================================================
// Construc route.
//================================================================
#ifdef SNAKE_SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
long
long
counter
=
-
1
;
// Note: route number 0 corresponds to the best route which is the last entry
// of *pSolutions.
for
(
auto
solution
=
pSolutions
->
end
()
-
1
;
solution
>=
pSolutions
->
begin
();
--
solution
)
{
++
counter
;
if
(
!*
solution
||
(
*
solution
)
->
Size
()
<=
1
)
{
std
::
stringstream
ss
;
ss
<<
par
.
errorString
<<
"Solution "
<<
counter
<<
"invalid."
<<
std
::
endl
;
par
.
errorString
=
ss
.
str
();
continue
;
}
// Iterate over all routes.
Solution
routeVector
;
for
(
std
::
size_t
vehicle
=
0
;
vehicle
<
numRuns
;
++
vehicle
)
{
if
(
!
routing
.
IsVehicleUsed
(
**
solution
,
vehicle
))
continue
;
// Create index list.
auto
index
=
routing
.
Start
(
vehicle
);
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
());
}
#ifdef SNAKE_DEBUG
// Print route.
std
::
cout
<<
"route "
<<
counter
<<
" route_idx.size() = "
<<
route_idx
.
size
()
<<
std
::
endl
;
std
::
cout
<<
"route: "
;
for
(
const
auto
&
idx
:
route_idx
)
{
std
::
cout
<<
idx
<<
", "
;
}
std
::
cout
<<
std
::
endl
;
#endif
if
(
route_idx
.
size
()
<
2
)
{
std
::
stringstream
ss
;
ss
<<
par
.
errorString
<<
"Error while assembling route (solution = "
<<
counter
<<
", run = "
<<
vehicle
<<
")."
<<
std
::
endl
;
par
.
errorString
=
ss
.
str
();
continue
;
}
// Assemble route.
Route
r
;
auto
&
path
=
r
.
path
;
auto
&
info
=
r
.
info
;
for
(
size_t
i
=
0
;
i
<
route_idx
.
size
()
-
1
;
++
i
)
{
size_t
nodeIndex0
=
route_idx
[
i
];
size_t
nodeIndex1
=
route_idx
[
i
+
1
];
const
auto
&
n2t0
=
nodeToTransectList
[
nodeIndex0
];
info
.
emplace_back
(
n2t0
.
transectsIndex
,
n2t0
.
reversed
);
// Copy transect to route.
const
auto
&
t
=
transects
[
n2t0
.
transectsIndex
];
if
(
n2t0
.
reversed
)
{
// transect reversal needed?
for
(
auto
it
=
t
.
end
()
-
1
;
it
>
t
.
begin
();
--
it
)
{
path
.
push_back
(
*
it
);
}
}
else
{
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
)
{
path
.
push_back
(
*
it
);
}
}
// Connect transects.
std
::
vector
<
size_t
>
idxList
;
if
(
!
shortestPathFromGraph
(
connectionGraph
,
nodeList
[
nodeIndex0
].
fromIndex
,
nodeList
[
nodeIndex1
].
toIndex
,
idxList
))
{
std
::
stringstream
ss
;
ss
<<
par
.
errorString
<<
"Error while assembling route (solution = "
<<
counter
<<
", run = "
<<
vehicle
<<
")."
<<
std
::
endl
;
par
.
errorString
=
ss
.
str
();
continue
;
}
if
(
i
!=
route_idx
.
size
()
-
2
)
{
idxList
.
pop_back
();
}
for
(
auto
idx
:
idxList
)
{
auto
p
=
int2Float
(
vertices
[
idx
]);
path
.
push_back
(
p
);
}
}
// Append last transect info.
const
auto
&
n2t0
=
nodeToTransectList
.
back
();
info
.
emplace_back
(
n2t0
.
transectsIndex
,
n2t0
.
reversed
);
if
(
path
.
size
()
<
2
||
info
.
size
()
<
2
)
{
std
::
stringstream
ss
;
ss
<<
par
.
errorString
<<
"Route empty (solution = "
<<
counter
<<
", run = "
<<
vehicle
<<
")."
<<
std
::
endl
;
par
.
errorString
=
ss
.
str
();
continue
;
}
routeVector
.
push_back
(
std
::
move
(
r
));
}
if
(
routeVector
.
size
()
>
0
)
{
solutionVector
.
push_back
(
std
::
move
(
routeVector
));
}
else
{
std
::
stringstream
ss
;
ss
<<
par
.
errorString
<<
"Solution "
<<
counter
<<
" empty."
<<
std
::
endl
;
par
.
errorString
=
ss
.
str
();
}
}
#ifdef SNAKE_SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"reconstruct route: "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
if
(
solutionVector
.
size
()
>
0
)
{
return
true
;
}
else
{
return
false
;
}
}
src/MeasurementComplexItem/routing.h
deleted
100644 → 0
View file @
29514d84
#ifndef ROUTING_H
#define ROUTING_H
#endif // ROUTING_H
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