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 += \
...
@@ -463,7 +463,6 @@ HEADERS += \
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
tileHelper
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
tileHelper
.
h
\
src
/
MeasurementComplexItem
/
routing
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
h
\
src
/
QmlControls
/
QmlUnitsConversion
.
h
\
src
/
QmlControls
/
QmlUnitsConversion
.
h
\
...
@@ -539,7 +538,6 @@ SOURCES += \
...
@@ -539,7 +538,6 @@ SOURCES += \
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
TaskDispatcher
.
cpp
\
src
/
MeasurementComplexItem
/
routing
.
cpp
\
src
/
Vehicle
/
VehicleEscStatusFactGroup
.
cc
\
src
/
Vehicle
/
VehicleEscStatusFactGroup
.
cc
\
src
/
MeasurementComplexItem
/
AreaData
.
cc
\
src
/
MeasurementComplexItem
/
AreaData
.
cc
\
src
/
api
/
QGCCorePlugin
.
cc
\
src
/
api
/
QGCCorePlugin
.
cc
\
...
...
src/MeasurementComplexItem/NemoInterface.cpp
View file @
7308bf55
...
@@ -7,7 +7,7 @@
...
@@ -7,7 +7,7 @@
#include "SettingsManager.h"
#include "SettingsManager.h"
#include "WimaSettings.h"
#include "WimaSettings.h"
#include <
shared_
mutex>
#include <mutex>
#include <QJsonArray>
#include <QJsonArray>
#include <QJsonObject>
#include <QJsonObject>
...
@@ -17,7 +17,6 @@
...
@@ -17,7 +17,6 @@
#include "GenericSingelton.h"
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "geometry/geometry.h"
#include "nemo_interface/FutureWatcher.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.h"
#include "nemo_interface/TaskDispatcher.h"
...
@@ -29,6 +28,30 @@
...
@@ -29,6 +28,30 @@
#include "ros_bridge/include/messages/nemo_msgs/tile_array.h"
#include "ros_bridge/include/messages/nemo_msgs/tile_array.h"
#include "rosbridge/rosbridge.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"
)
QGC_LOGGING_CATEGORY
(
NemoInterfaceLog
,
"NemoInterfaceLog"
)
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
...
@@ -36,10 +59,32 @@ QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
...
@@ -36,10 +59,32 @@ QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define RESTART_RETRY_INTERVAl 2000 // ms
#define RESTART_RETRY_INTERVAl 2000 // ms
#define SYNC_INTERVAL 10000 // ms
#define SYNC_INTERVAL 10000 // ms
#define SYNC_RETRY_INTERVAL 2000 // ms
#define SYNC_RETRY_INTERVAL 2000 // ms
static
constexpr
auto
maxResponseTime
=
std
::
chrono
::
milliseconds
(
10000
);
static
constexpr
auto
maxResponseTime
=
std
::
chrono
::
milliseconds
(
10000
);
static
const
char
*
progressTopic
=
"/nemo/progress"
;
static
char
const
*
progressTopic
=
"/nemo/progress"
;
static
const
char
*
heartbeatTopic
=
"/nemo/heartbeat"
;
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
hrc
=
std
::
chrono
::
high_resolution_clock
;
using
ROSBridgePtr
=
std
::
shared_ptr
<
Rosbridge
>
;
using
ROSBridgePtr
=
std
::
shared_ptr
<
Rosbridge
>
;
...
@@ -51,8 +96,7 @@ typedef std::map<QString, std::shared_ptr<Tile>> TileMap;
...
@@ -51,8 +96,7 @@ typedef std::map<QString, std::shared_ptr<Tile>> TileMap;
typedef
std
::
map
<
QString
,
std
::
shared_ptr
<
const
Tile
>>
TileMapConst
;
typedef
std
::
map
<
QString
,
std
::
shared_ptr
<
const
Tile
>>
TileMapConst
;
typedef
ros_bridge
::
messages
::
nemo_msgs
::
heartbeat
::
Heartbeat
Heartbeat
;
typedef
ros_bridge
::
messages
::
nemo_msgs
::
heartbeat
::
Heartbeat
Heartbeat
;
typedef
nemo_interface
::
TaskDispatcher
Dispatcher
;
typedef
nemo_interface
::
TaskDispatcher
Dispatcher
;
typedef
nemo_interface
::
FutureWatcher
<
QVariant
,
std
::
shared_future
>
typedef
std
::
unique_lock
<
std
::
mutex
>
Lock
;
FutureWatcher
;
class
NemoInterface
::
Impl
{
class
NemoInterface
::
Impl
{
enum
class
STATE
{
enum
class
STATE
{
...
@@ -62,6 +106,7 @@ class NemoInterface::Impl {
...
@@ -62,6 +106,7 @@ class NemoInterface::Impl {
TRY_SETUP
,
TRY_SETUP
,
USER_SYNC
,
USER_SYNC
,
SYS_SYNC
,
SYS_SYNC
,
SYNC_ERROR
,
READY
,
READY
,
WEBSOCKET_TIMEOUT
,
WEBSOCKET_TIMEOUT
,
HEARTBEAT_TIMEOUT
HEARTBEAT_TIMEOUT
...
@@ -99,96 +144,84 @@ public:
...
@@ -99,96 +144,84 @@ public:
const
QString
&
warningString
()
const
;
const
QString
&
warningString
()
const
;
private:
private:
void
_checkVersion
();
void
_doSetup
();
void
_subscribeProgressTopic
();
void
_doActionNotSafe
();
void
_subscribeHearbeatTopic
();
void
_synchronize
();
void
_doAction
();
void
_trySynchronize
();
void
_synchronizeIfNeccessary
();
void
_tryRestart
();
void
_tryRestart
();
bool
_isSynchronizedNotSafe
()
const
;
bool
_isSynchronized
()
const
;
bool
_isSynchronized
()
const
;
bool
_userSync
()
const
;
// thread safe
void
_onHeartbeatTimeout
();
bool
_sysSync
()
const
;
// thread safe
void
_onFutureWatcherFinished
();
// thread safe
void
_onHeartbeatTimeout
();
// thread safe
void
_onRosbridgeStateChanged
();
void
_onRosbridgeStateChanged
();
bool
_updateProgress
(
const
ProgressArray
&
pArray
);
// called from dispatcher thread!
void
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
hb
);
QVariant
_callAddTiles
(
void
_setInfoStringNotSafe
(
const
QString
&
info
);
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
);
void
_setWarningStringNotSafe
(
const
QString
&
warning
);
// 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
);
void
_setInfoString
(
const
QString
&
info
);
void
_setInfoString
(
const
QString
&
info
);
void
_setWarningString
(
const
QString
&
warning
);
void
_setWarningString
(
const
QString
&
warning
);
bool
_setState
(
STATE
newState
);
// not thread safe
// state suff
bool
_userSync
()
const
;
static
bool
_ready
(
STATE
s
);
bool
_sysSync
()
const
;
static
bool
_userSync
(
STATE
s
);
bool
_setStateNotSafe
(
STATE
newState
);
static
bool
_sysSync
(
STATE
s
);
static
bool
_readyNotSafe
(
STATE
s
);
static
bool
_running
(
STATE
s
);
static
bool
_userSyncNotSafe
(
STATE
s
);
static
bool
_sysSyncNotSafe
(
STATE
s
);
static
bool
_runningNotSafe
(
STATE
s
);
static
NemoInterface
::
STATUS
_status
(
STATE
state
);
static
NemoInterface
::
STATUS
_status
(
STATE
state
);
static
QString
_toString
(
STATE
s
);
static
QString
_toString
(
STATE
s
);
static
QString
_toString
(
NemoInterface
::
STATUS
s
);
static
QString
_toString
(
NemoInterface
::
STATUS
s
);
static
QString
_localVersion
;
// impl functions
QString
_remoteVersion
;
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
;
// thread safe members
std
::
atomic_bool
_versionOK
;
std
::
atomic_bool
_progressTopicOK
;
std
::
atomic_bool
_heartbeatTopicOK
;
std
::
atomic
<
CALL_NAME
>
_lastCall
;
ROSBridgePtr
_pRosbridge
;
ROSBridgePtr
_pRosbridge
;
Dispatcher
_dispatcher
;
// protected by mutex
mutable
std
::
mutex
_m
;
STATE
_state
;
TileMap
_remoteTiles
;
TileMap
_remoteTiles
;
TileMapConst
_localTiles
;
TileMapConst
_localTiles
;
NemoInterface
*
const
_parent
;
Dispatcher
_dispatcher
;
QString
_infoString
;
QString
_infoString
;
QString
_warningString
;
QString
_warningString
;
// Members belonging to _parent->thread()
QTimer
_timeoutTimer
;
QTimer
_timeoutTimer
;
QTimer
_syncTimer
;
QTimer
_syncTimer
;
QTimer
_restartTimer
;
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
>
;
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
static
StatusMap
statusMap
{
static
StatusMap
statusMap
{
...
@@ -206,8 +239,7 @@ static StatusMap statusMap{
...
@@ -206,8 +239,7 @@ static StatusMap statusMap{
NemoInterface
::
STATUS
::
WEBSOCKET_DETECTED
,
"Websocket Detected"
)};
NemoInterface
::
STATUS
::
WEBSOCKET_DETECTED
,
"Websocket Detected"
)};
NemoInterface
::
Impl
::
Impl
(
NemoInterface
*
p
)
NemoInterface
::
Impl
::
Impl
(
NemoInterface
*
p
)
:
_state
(
STATE
::
STOPPED
),
_versionOK
(
false
),
_progressTopicOK
(
false
),
:
_parent
(
p
),
_state
(
STATE
::
STOPPED
)
{
_heartbeatTopicOK
(
false
),
_parent
(
p
)
{
// ROS Bridge.
// ROS Bridge.
WimaSettings
*
wimaSettings
=
WimaSettings
*
wimaSettings
=
...
@@ -224,6 +256,7 @@ NemoInterface::Impl::Impl(NemoInterface *p)
...
@@ -224,6 +256,7 @@ NemoInterface::Impl::Impl(NemoInterface *p)
this
->
start
();
this
->
start
();
}
}
};
};
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
setConnectionString
);
setConnectionString
();
setConnectionString
();
...
@@ -235,29 +268,31 @@ NemoInterface::Impl::Impl(NemoInterface *p)
...
@@ -235,29 +268,31 @@ NemoInterface::Impl::Impl(NemoInterface *p)
connect
(
this
->
_pRosbridge
.
get
(),
&
Rosbridge
::
stateChanged
,
this
->
_parent
,
connect
(
this
->
_pRosbridge
.
get
(),
&
Rosbridge
::
stateChanged
,
this
->
_parent
,
[
this
]
{
this
->
_onRosbridgeStateChanged
();
});
[
this
]
{
this
->
_onRosbridgeStateChanged
();
});
connect
(
&
this
->
_futureWatcher
,
&
FutureWatcher
::
finished
,
this
->
_parent
,
[
this
]
{
this
->
_onFutureWatcherFinished
();
});
connect
(
&
this
->
_restartTimer
,
&
QTimer
::
timeout
,
this
->
_parent
,
connect
(
&
this
->
_restartTimer
,
&
QTimer
::
timeout
,
this
->
_parent
,
[
this
]
{
this
->
_tryRestart
();
});
[
this
]
{
this
->
_tryRestart
();
});
connect
(
&
this
->
_syncTimer
,
&
QTimer
::
timeout
,
this
->
_parent
,
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
();
}
NemoInterface
::
Impl
::~
Impl
()
{
this
->
_pRosbridge
->
stop
();
}
void
NemoInterface
::
Impl
::
start
()
{
void
NemoInterface
::
Impl
::
start
()
{
if
(
!
running
())
{
Lock
lk
(
this
->
_m
);
this
->
_setState
(
STATE
::
START_BRIDGE
);
if
(
!
_runningNotSafe
(
this
->
_state
))
{
this
->
_doAction
();
this
->
_setStateNotSafe
(
STATE
::
START_BRIDGE
);
this
->
_doActionNotSafe
();
}
}
}
}
void
NemoInterface
::
Impl
::
stop
()
{
void
NemoInterface
::
Impl
::
stop
()
{
if
(
running
())
{
Lock
lk
(
this
->
_m
);
this
->
_setState
(
STATE
::
STOPPED
);
if
(
_runningNotSafe
(
this
->
_state
))
{
this
->
_doAction
();
this
->
_setStateNotSafe
(
STATE
::
STOPPED
);
this
->
_doActionNotSafe
();
}
}
}
}
...
@@ -272,6 +307,8 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
...
@@ -272,6 +307,8 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
// copy unknown tiles
// copy unknown tiles
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
Lock
lk
(
this
->
_m
);
for
(
const
auto
*
pTile
:
tileArray
)
{
for
(
const
auto
*
pTile
:
tileArray
)
{
auto
id
=
pTile
->
id
();
auto
id
=
pTile
->
id
();
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
...
@@ -288,38 +325,36 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
...
@@ -288,38 +325,36 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
}
}
}
}
if
(
pTileArray
->
size
()
>
0
)
{
if
(
pTileArray
->
size
()
>
0
)
{
lk
.
unlock
();
emit
this
->
_parent
->
tilesChanged
();
emit
this
->
_parent
->
tilesChanged
();
lk
.
lock
();
}
}
// ready for send?
// ready for send?
if
(
pTileArray
->
size
()
>
0
&&
(
this
->
ready
()
||
this
->
_userSync
()))
{
if
(
pTileArray
->
size
()
>
0
&&
(
this
->
_readyNotSafe
(
this
->
_state
)
||
this
->
_userSyncNotSafe
(
this
->
_state
)))
{
this
->
_setState
(
STATE
::
USER_SYNC
);
this
->
_doAction
();
this
->
_setStateNotSafe
(
STATE
::
USER_SYNC
);
this
->
_doActionNotSafe
();
// create add tiles command.
lk
.
unlock
();
auto
pTask
=
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callAddTiles
,
this
,
pTileArray
));
// create task.
auto
pTask
=
std
::
make_unique
<
Task
>
([
this
,
pTileArray
,
pIdArray
]
{
// dispatch command.
auto
ret
=
this
->
_addTilesImpl
(
pTileArray
,
pIdArray
);
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
addFuture
=
ret
.
share
();
if
(
ret
)
{
Lock
lk
(
this
->
_m
);
// create get progress cmd.
if
(
this
->
_isSynchronizedNotSafe
())
{
pTask
=
std
::
make_unique
<
Task
>
([
this
,
addFuture
,
pIdArray
]
{
this
->
_setStateNotSafe
(
STATE
::
READY
);
addFuture
.
wait
();
this
->
_doActionNotSafe
();
if
(
addFuture
.
get
().
toBool
())
{
}
return
this
->
_callGetProgress
(
pIdArray
);
}
else
{
return
QVariant
(
false
);
}
}
return
ret
;
});
});
// dispatch command.
// dispatch command.
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
progressFuture
=
ret
.
share
();
return
ret
.
share
();
_futureWatcher
.
setFuture
(
progressFuture
);
return
progressFuture
;
}
}
}
}
...
@@ -338,6 +373,8 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
...
@@ -338,6 +373,8 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
// copy known ids
// copy known ids
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
Lock
lk
(
this
->
_m
);
for
(
const
auto
&
id
:
idArray
)
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
Q_ASSERT
(
it
!=
_localTiles
.
end
());
Q_ASSERT
(
it
!=
_localTiles
.
end
());
...
@@ -349,23 +386,37 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
...
@@ -349,23 +386,37 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
}
}
}
}
if
(
pIdArray
->
size
()
>
0
)
{
if
(
pIdArray
->
size
()
>
0
)
{
lk
.
unlock
();
emit
this
->
_parent
->
tilesChanged
();
emit
this
->
_parent
->
tilesChanged
();
lk
.
lock
();
}
}
// ready for send?
// 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
->
_setStateNotSafe
(
STATE
::
USER_SYNC
);
this
->
_doAction
();
this
->
_doActionNotSafe
();
lk
.
unlock
();
// create command.
// create command.
auto
cmd
=
std
::
make_unique
<
Task
>
(
auto
cmd
=
std
::
make_unique
<
Task
>
([
this
,
pIdArray
]
{
std
::
bind
(
&
Impl
::
_callRemoveTiles
,
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.
// dispatch command and return.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
cmd
));
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
cmd
));
auto
sfut
=
ret
.
share
();
auto
sfut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
sfut
);
return
sfut
;
return
sfut
;
}
}
}
}
...
@@ -381,26 +432,40 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
...
@@ -381,26 +432,40 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
// qDebug() << "clearTiles called";
// qDebug() << "clearTiles called";
// clear local tiles (_localTiles)
// clear local tiles (_localTiles)
Lock
lk
(
this
->
_m
);
if
(
!
_localTiles
.
empty
())
{
if
(
!
_localTiles
.
empty
())
{
this
->
_localTiles
.
clear
();
this
->
_localTiles
.
clear
();
lk
.
unlock
();
emit
this
->
_parent
->
tilesChanged
();
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
->
_setStateNotSafe
(
STATE
::
USER_SYNC
);
this
->
_doAction
();
this
->
_doActionNotSafe
();
lk
.
unlock
();
// create command.
// create command.
auto
pTask
=
auto
pTask
=
std
::
make_unique
<
Task
>
([
this
]
{
std
::
make_unique
<
Task
>
(
std
::
bind
(
&
Impl
::
_callClearTiles
,
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.
// dispatch command and return.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
sfut
=
ret
.
share
();
return
ret
.
share
();
_futureWatcher
.
setFuture
(
sfut
);
return
sfut
;
}
else
{
}
else
{
lk
.
unlock
();
std
::
promise
<
QVariant
>
p
;
std
::
promise
<
QVariant
>
p
;
p
.
set_value
(
QVariant
(
false
));
p
.
set_value
(
QVariant
(
false
));
return
p
.
get_future
();
return
p
.
get_future
();
...
@@ -410,6 +475,7 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
...
@@ -410,6 +475,7 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
TileArray
NemoInterface
::
Impl
::
getTiles
(
const
IDArray
&
idArray
)
const
{
TileArray
NemoInterface
::
Impl
::
getTiles
(
const
IDArray
&
idArray
)
const
{
TileArray
tileArray
;
TileArray
tileArray
;
Lock
lk
(
this
->
_m
);
if
(
this
->
ready
())
{
if
(
this
->
ready
())
{
for
(
const
auto
&
id
:
idArray
)
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_remoteTiles
.
find
(
id
);
const
auto
it
=
_remoteTiles
.
find
(
id
);
...
@@ -440,6 +506,7 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) const {
...
@@ -440,6 +506,7 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) const {
TileArray
NemoInterface
::
Impl
::
getAllTiles
()
const
{
TileArray
NemoInterface
::
Impl
::
getAllTiles
()
const
{
TileArray
tileArray
;
TileArray
tileArray
;
Lock
lk
(
this
->
_m
);
if
(
this
->
ready
())
{
if
(
this
->
ready
())
{
for
(
const
auto
&
entry
:
_remoteTiles
)
{
for
(
const
auto
&
entry
:
_remoteTiles
)
{
auto
pTile
=
entry
.
second
;
auto
pTile
=
entry
.
second
;
...
@@ -467,6 +534,7 @@ TileArray NemoInterface::Impl::getAllTiles() const {
...
@@ -467,6 +534,7 @@ TileArray NemoInterface::Impl::getAllTiles() const {
LogicalArray
NemoInterface
::
Impl
::
containsTiles
(
const
IDArray
&
idArray
)
const
{
LogicalArray
NemoInterface
::
Impl
::
containsTiles
(
const
IDArray
&
idArray
)
const
{
LogicalArray
logicalArray
;
LogicalArray
logicalArray
;
Lock
lk
(
this
->
_m
);
for
(
const
auto
&
id
:
idArray
)
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
&
it
=
_localTiles
.
find
(
id
);
const
auto
&
it
=
_localTiles
.
find
(
id
);
logicalArray
.
append
(
it
!=
_localTiles
.
end
());
logicalArray
.
append
(
it
!=
_localTiles
.
end
());
...
@@ -475,14 +543,22 @@ LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) const {
...
@@ -475,14 +543,22 @@ LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) const {
return
logicalArray
;
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
NemoInterface
::
Impl
::
getProgress
()
const
{
ProgressArray
progressArray
;
ProgressArray
progressArray
;
if
(
this
->
_isSynchronized
())
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_isSynchronizedNotSafe
())
{
for
(
const
auto
&
entry
:
_remoteTiles
)
{
for
(
const
auto
&
entry
:
_remoteTiles
)
{
progressArray
.
append
(
progressArray
.
append
(
LabeledProgress
{
entry
.
second
->
progress
(),
entry
.
second
->
id
()});
LabeledProgress
{
entry
.
second
->
progress
(),
entry
.
second
->
id
()});
...
@@ -500,7 +576,8 @@ ProgressArray NemoInterface::Impl::getProgress() const {
...
@@ -500,7 +576,8 @@ ProgressArray NemoInterface::Impl::getProgress() const {
ProgressArray
NemoInterface
::
Impl
::
getProgress
(
const
IDArray
&
idArray
)
const
{
ProgressArray
NemoInterface
::
Impl
::
getProgress
(
const
IDArray
&
idArray
)
const
{
ProgressArray
progressArray
;
ProgressArray
progressArray
;
if
(
this
->
_isSynchronized
())
{
Lock
lk
(
this
->
_m
);
if
(
this
->
_isSynchronizedNotSafe
())
{
for
(
const
auto
&
id
:
idArray
)
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
_remoteTiles
.
find
(
id
);
const
auto
it
=
_remoteTiles
.
find
(
id
);
if
(
it
!=
_remoteTiles
.
end
())
{
if
(
it
!=
_remoteTiles
.
end
())
{
...
@@ -522,173 +599,178 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) const {
...
@@ -522,173 +599,178 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) const {
}
}
NemoInterface
::
STATUS
NemoInterface
::
Impl
::
status
()
const
{
NemoInterface
::
STATUS
NemoInterface
::
Impl
::
status
()
const
{
Lock
lk
(
this
->
_m
);
return
_status
(
this
->
_state
);
return
_status
(
this
->
_state
);
}
}
bool
NemoInterface
::
Impl
::
running
()
const
{
return
_running
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
running
()
const
{
Lock
lk1
(
this
->
_m
);
bool
NemoInterface
::
Impl
::
ready
()
const
{
return
_ready
(
this
->
_state
.
load
());
}
return
_runningNotSafe
(
this
->
_state
);
}
bool
NemoInterface
::
Impl
::
_sysSync
()
const
{
return
_sysSync
(
this
->
_state
);
}
void
NemoInterface
::
Impl
::
_onFutureWatcherFinished
()
{
bool
NemoInterface
::
Impl
::
ready
()
const
{
if
(
this
->
ready
()
||
this
->
_userSync
()
||
this
->
_sysSync
())
{
Lock
lk1
(
this
->
_m
);
static
long
tries
=
0
;
return
_readyNotSafe
(
this
->
_state
);
auto
lastTransactionSuccessfull
=
_futureWatcher
.
result
().
toBool
();
}
if
(
!
lastTransactionSuccessfull
)
{
++
tries
;
qCDebug
(
NemoInterfaceLog
)
<<
"last transaction unsuccessfull: "
<<
_toString
(
_lastCall
);
if
(
tries
<
5
)
{
bool
NemoInterface
::
Impl
::
_sysSync
()
const
{
QTimer
::
singleShot
(
SYNC_RETRY_INTERVAL
,
this
->
_parent
,
Lock
lk1
(
this
->
_m
);
[
this
]
{
this
->
_trySynchronize
();
});
return
_sysSyncNotSafe
(
this
->
_state
);
}
else
{
_setWarningString
(
"The last 5 transactions failed! Please check the "
"connection and consider reseting the connection."
);
tries
=
0
;
}
}
else
{
tries
=
0
;
}
}
}
}
void
NemoInterface
::
Impl
::
_onHeartbeatTimeout
()
{
void
NemoInterface
::
Impl
::
_onHeartbeatTimeout
()
{
this
->
_setState
(
STATE
::
HEARTBEAT_TIMEOUT
);
Lock
lk1
(
this
->
_m
);
this
->
_doAction
();
this
->
_setStateNotSafe
(
STATE
::
HEARTBEAT_TIMEOUT
);
this
->
_doActionNotSafe
();
}
}
void
NemoInterface
::
Impl
::
_onRosbridgeStateChanged
()
{
void
NemoInterface
::
Impl
::
_onRosbridgeStateChanged
()
{
auto
state
=
this
->
_pRosbridge
->
state
();
auto
rbState
=
this
->
_pRosbridge
->
state
();
if
(
state
==
Rosbridge
::
STATE
::
CONNECTED
)
{
if
(
rbState
==
Rosbridge
::
STATE
::
CONNECTED
)
{
Lock
lk1
(
this
->
_m
);
if
(
this
->
_state
==
STATE
::
START_BRIDGE
||
if
(
this
->
_state
==
STATE
::
START_BRIDGE
||
this
->
_state
==
STATE
::
WEBSOCKET_TIMEOUT
)
{
this
->
_state
==
STATE
::
WEBSOCKET_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_DETECTED
);
this
->
_setState
NotSafe
(
STATE
::
WEBSOCKET_DETECTED
);
this
->
_doAction
();
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
||
if
(
this
->
_state
==
STATE
::
TRY_SETUP
||
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
WEBSOCKET_DETECTED
||
this
->
_state
==
STATE
::
WEBSOCKET_DETECTED
||
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_TIMEOUT
);
this
->
_setState
NotSafe
(
STATE
::
WEBSOCKET_TIMEOUT
);
this
->
_doAction
();
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
{
const
QString
&
NemoInterface
::
Impl
::
warningString
()
const
{
Lock
lk1
(
this
->
_m
);
return
_warningString
;
return
_warningString
;
}
}
void
NemoInterface
::
Impl
::
_updateProgress
(
std
::
shared_ptr
<
ProgressArray
>
pArray
,
bool
NemoInterface
::
Impl
::
_updateProgress
(
const
ProgressArray
&
pArray
)
{
std
::
promise
<
bool
>
promise
)
{
// qDebug() << "_updateProgress called";
ProgressArray
copy
;
bool
error
=
false
;
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
());
auto
it
=
_remoteTiles
.
find
(
itLP
->
id
());
if
(
Q_LIKELY
(
it
!=
_remoteTiles
.
end
()))
{
if
(
Q_LIKELY
(
it
!=
_remoteTiles
.
end
()))
{
it
->
second
->
setProgress
(
itLP
->
progress
());
it
->
second
->
setProgress
(
itLP
->
progress
());
++
itLP
;
copy
.
push_back
(
*
itLP
)
;
}
else
{
}
else
{
qCDebug
(
NemoInterfaceLog
)
qCDebug
(
NemoInterfaceLog
)
<<
"_updateProgress(): tile with id "
<<
itLP
->
id
()
<<
" not found."
;
<<
"_updateProgress(): tile with id "
<<
itLP
->
id
()
<<
" not found."
;
itLP
=
pArray
->
erase
(
itLP
);
error
=
true
;
error
=
true
;
}
}
}
}
lk1
.
unlock
();
if
(
pArray
->
size
()
>
0
)
{
if
(
copy
.
size
()
>
0
)
{
emit
_parent
->
progressChanged
(
*
pArra
y
);
emit
_parent
->
progressChanged
(
cop
y
);
}
}
promise
.
set_value
(
!
error
)
;
return
!
error
;
}
}
void
NemoInterface
::
Impl
::
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
hb
,
void
NemoInterface
::
Impl
::
_onHeartbeatReceived
(
const
QNemoHeartbeat
&
)
{
std
::
promise
<
bool
>
promise
)
{
INVM
(
this
->
_parent
,
_lastHeartbeat
=
hb
;
([
this
]()
mutable
{
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
}))
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
if
(
this
->
_state
==
STATE
::
TRY_SETUP
)
{
Lock
lk
(
this
->
_m
);
this
->
_setState
(
STATE
::
READY
);
if
(
this
->
_state
==
STATE
::
TRY_SETUP
||
this
->
_doAction
();
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
}
else
if
(
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setStateNotSafe
(
STATE
::
READY
);
this
->
_setState
(
STATE
::
READY
);
this
->
_doActionNotSafe
();
this
->
_doAction
();
}
}
promise
.
set_value
(
true
);
}
}
void
NemoInterface
::
Impl
::
_setInfoString
(
const
QString
&
info
)
{
void
NemoInterface
::
Impl
::
_setInfoString
NotSafe
(
const
QString
&
info
)
{
if
(
_infoString
!=
info
)
{
if
(
_infoString
!=
info
)
{
_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
)
{
if
(
_warningString
!=
warning
)
{
_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
]
{
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
]
{
// wait for service
auto
rb
=
this
->
_pRosbridge
;
std
::
future
<
bool
>
fut
;
auto
&
disp
=
this
->
_dispatcher
;
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
());
// call service
// check if required services are available
return
this
->
_callGetVersion
();
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
available
=
rb
->
waitForService
(
cc
,
cond
);
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
]
{
if
(
!
available
)
{
// wait for service
return
QVariant
(
false
);
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
;
}
}
}
}
while
(
!
fut
.
get
());
}
// subscribe
// check if version is compatible{
this
->
_pRosbridge
->
subscribeTopic
(
progressTopic
,
[
this
](
this
->
_setInfoString
(
"Checking version."
);
const
QJsonObject
&
o
)
{
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
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
progressArray
;
progressArray
;
if
(
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
fromJson
(
if
(
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
fromJson
(
...
@@ -707,167 +789,161 @@ void NemoInterface::Impl::_subscribeProgressTopic() {
...
@@ -707,167 +789,161 @@ void NemoInterface::Impl::_subscribeProgressTopic() {
}
}
if
(
rangeError
)
{
if
(
rangeError
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress progress out "
qCWarning
(
NemoInterfaceLog
)
<<
progressTopic
<<
" progress out "
"of range, value was set to: "
"of range, value was set to: "
<<
lp
.
progress
();
<<
lp
.
progress
();
}
}
}
}
auto
ret
=
this
->
_updateProgress
(
progressArray
.
progress_array
());
auto
p
=
std
::
make_shared
<
ProgressArray
>
();
if
(
!
ret
)
*
p
=
std
::
move
(
progressArray
.
progress_array
());
this
->
_setWarningString
(
"Progress update failed."
);
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
();
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress not able to "
qCWarning
(
NemoInterfaceLog
)
<<
progressTopic
<<
" not able to "
"create ProgressArray form json: "
"create ProgressArray form json: "
<<
o
;
<<
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
;
using
namespace
ros_bridge
::
messages
;
this
->
_pRosbridge
->
subscribeTopic
(
rb
->
subscribeTopic
(
heartbeatTopic
,
[
this
](
const
QJsonObject
&
o
)
{
heartbeatTopic
,
[
this
](
const
QJsonObject
&
o
)
{
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
o
,
heartbeat
))
{
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
o
,
heartbeat
))
{
this
->
_onHeartbeatReceived
(
heartbeat
);
std
::
promise
<
bool
>
promise
;
}
else
{
auto
future
=
promise
.
get_future
();
qCWarning
(
NemoInterfaceLog
)
<<
heartbeatTopic
bool
value
=
QMetaObject
::
invokeMethod
(
<<
" not able to "
this
->
_parent
,
"create Heartbeat form json: "
[
this
,
heartbeat
,
promise
=
std
::
move
(
promise
)]()
mutable
{
<<
o
;
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
);
// now ready
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
}))
_setInfoString
(
""
);
_setWarningString
(
""
);
return
QVariant
(
true
);
return
QVariant
(
true
);
});
});
this
->
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
f
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
Q_UNUSED
(
f
);
}
}
void
NemoInterface
::
Impl
::
_trySynchronize
()
{
void
NemoInterface
::
Impl
::
_synchronize
()
{
if
((
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
SYS_SYNC
||
Lock
lk
(
this
->
_m
);
this
->
_state
==
STATE
::
USER_SYNC
)
&&
if
(
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
SYNC_ERROR
)
{
!
_isSynchronized
())
{
if
(
!
_dispatcher
.
idle
())
{
_setStateNotSafe
(
STATE
::
SYS_SYNC
);
QTimer
::
singleShot
(
SYNC_RETRY_INTERVAL
,
this
->
_parent
,
_doActionNotSafe
();
[
this
]
{
this
->
_trySynchronize
();
});
qCWarning
(
NemoInterfaceLog
)
<<
"synchronization defered"
;
return
;
}
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
);
// compare with pTileArray
this
->
_doAction
();
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.
if
(
!
synced
)
{
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
(
auto
success
=
this
->
_clearTilesImpl
();
std
::
bind
(
&
Impl
::
_callClearTiles
,
this
));
if
(
!
success
)
{
Lock
lk
(
this
->
_m
);
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
_doActionNotSafe
();
return
QVariant
(
false
);
}
// dispatch command.
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
Q_ASSERT
(
_dispatcher
.
pendingTasks
()
==
0
);
for
(
auto
&&
pTile
:
*
pTileArray
)
{
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
pIdArray
->
push_back
(
pTile
->
id
(
));
auto
clearFuture
=
ret
.
share
();
}
// create tile array.
success
=
this
->
_addTilesImpl
(
pTileArray
,
pIdArray
);
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
for
(
const
auto
&
pair
:
_localTiles
)
{
pTileArray
->
push_back
(
pair
.
second
);
}
// create addTiles cmd.
if
(
!
success
)
{
pTask
=
Lock
lk
(
this
->
_m
);
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
,
pTileArray
,
clearFuture
]
{
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
clearFuture
.
wait
();
_doActionNotSafe
();
if
(
clearFuture
.
get
().
toBool
())
{
return
QVariant
(
false
);
return
this
->
_callAddTiles
(
pTileArray
);
}
}
else
{
}
else
{
return
QVariant
(
false
);
// 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.
if
(
!
ret
)
{
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
Lock
lk
(
this
->
_m
);
auto
addFuture
=
ret
.
share
();
this
->
_setStateNotSafe
(
STATE
::
SYNC_ERROR
);
this
->
_doActionNotSafe
();
// create GetAllProgress cmd.
lk
.
unlock
();
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
,
addFuture
]
{
this
->
_setWarningString
(
"Getting progress failed."
);
addFuture
.
wait
();
qCDebug
(
NemoInterfaceLog
)
if
(
addFuture
.
get
().
toBool
())
{
<<
"_addTilesImpl(): _updateProgress failed: unknown id."
;
return
this
->
_callGetAllProgress
();
return
QVariant
(
false
);
}
else
{
}
return
QVariant
(
false
);
}
}
});
// dispatch command.
// check if local versions are still synced (user could habe modified
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
// _localTiles).
_futureWatcher
.
setFuture
(
ret
.
share
());
Lock
lk
(
this
->
_m
);
}
if
(
_isSynchronizedNotSafe
())
{
}
_setStateNotSafe
(
STATE
::
READY
);
_doActionNotSafe
();
}
else
{
INVM
(
this
->
_parent
,
[
this
]
{
this
->
_synchronize
();
})
}
lk
.
unlock
();
void
NemoInterface
::
Impl
::
_synchronizeIfNeccessary
()
{
return
QVariant
(
true
);
if
(
_dispatcher
.
idle
())
{
});
// create getAllTiles cmd.
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
(
[
this
]
{
return
this
->
_callGetAllTiles
();
});
// dispatch command.
// dispatch command.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
fut
=
ret
.
share
();
Q_UNUSED
(
ret
);
_futureWatcher
.
setFuture
(
fut
);
INVM
(
this
->
_parent
,
[
this
]
{
this
->
_syncTimer
.
start
(
SYNC_INTERVAL
);
})
_syncTimer
.
start
(
SYNC_INTERVAL
);
}
else
{
}
else
{
_syncTimer
.
start
(
SYNC_RETRY_INTERVAL
);
INVM
(
this
->
_parent
,
[
this
]
{
this
->
_syncTimer
.
start
(
SYNC_RETRY_INTERVAL
);
})
}
}
}
}
...
@@ -884,88 +960,103 @@ void NemoInterface::Impl::_tryRestart() {
...
@@ -884,88 +960,103 @@ void NemoInterface::Impl::_tryRestart() {
}
}
}
}
bool
NemoInterface
::
Impl
::
_isSynchronized
()
const
{
bool
NemoInterface
::
Impl
::
_isSynchronized
NotSafe
()
const
{
return
_localTiles
.
size
()
==
_remoteTiles
.
size
()
&&
return
_localTiles
.
size
()
==
_remoteTiles
.
size
()
&&
std
::
equal
(
std
::
equal
(
_localTiles
.
begin
(),
_localTiles
.
end
(),
_remoteTiles
.
begin
(),
_localTiles
.
begin
(),
_localTiles
.
end
(),
_remoteTiles
.
begin
(),
[](
const
auto
&
a
,
const
auto
&
b
)
{
return
a
.
first
==
b
.
first
;
});
[](
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
;
static
bool
resetDone
=
false
;
switch
(
this
->
_state
)
{
switch
(
this
->
_state
)
{
case
STATE
:
:
STOPPED
:
case
STATE
:
:
STOPPED
:
{
_setWarningString
(
""
);
std
::
promise
<
void
>
p
;
_setInfoString
(
""
);
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_timeoutTimer
.
stop
();
this
->
_timeoutTimer
.
stop
();
this
->
_restartTimer
.
stop
();
this
->
_restartTimer
.
stop
();
this
->
_syncTimer
.
stop
();
this
->
_syncTimer
.
stop
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
}))
if
(
this
->
_pRosbridge
->
state
()
!=
Rosbridge
::
STATE
::
STOPPED
)
{
if
(
this
->
_pRosbridge
->
state
()
!=
Rosbridge
::
STATE
::
STOPPED
)
{
this
->
_pRosbridge
->
stop
();
this
->
_pRosbridge
->
stop
();
}
}
_versionOK
=
false
;
_dispatcher
.
stop
();
_progressTopicOK
=
false
;
_dispatcher
.
clear
();
_heartbeatTopicOK
=
false
;
break
;
_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
->
_pRosbridge
->
start
();
this
->
_restartTimer
.
start
(
RESTART_INTERVAl
);
}
break
;
break
;
case
STATE
:
:
WEBSOCKET_DETECTED
:
case
STATE
:
:
WEBSOCKET_DETECTED
:
resetDone
=
false
;
resetDone
=
false
;
this
->
_setState
(
STATE
::
TRY_SETUP
);
this
->
_setState
NotSafe
(
STATE
::
TRY_SETUP
);
this
->
_doAction
();
this
->
_doAction
NotSafe
();
break
;
break
;
case
STATE
:
:
TRY_SETUP
:
case
STATE
:
:
TRY_SETUP
:
if
(
!
_versionOK
)
{
this
->
_doSetup
();
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
);
break
;
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
:
:
USER_SYNC
:
case
STATE
:
:
SYS_SYNC
:
case
STATE
:
:
SYS_SYNC
:
case
STATE
:
:
SYNC_ERROR
:
break
;
break
;
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
{
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_syncTimer
.
stop
();
}))
this
->
_syncTimer
.
stop
();
break
;
}
break
;
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
{
INVM
(
this
->
_parent
,
([
this
]()
mutable
{
this
->
_timeoutTimer
.
stop
();
this
->
_syncTimer
.
stop
();
}))
if
(
!
resetDone
)
{
if
(
!
resetDone
)
{
resetDone
=
true
;
resetDone
=
true
;
this
->
_pRosbridge
->
stop
();
this
->
_pRosbridge
->
stop
();
this
->
_pRosbridge
->
start
();
this
->
_pRosbridge
->
start
();
}
}
this
->
_timeoutTimer
.
stop
();
_dispatcher
.
stop
();
this
->
_syncTimer
.
stop
();
_dispatcher
.
clear
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
_versionOK
=
false
;
_setInfoStringNotSafe
(
""
);
_progressTopicOK
=
false
;
_setWarningStringNotSafe
(
""
);
_heartbeatTopicOK
=
false
;
}
break
;
break
;
};
};
}
}
QVariant
NemoInterface
::
Impl
::
_callAddTiles
(
bool
NemoInterface
::
Impl
::
_callAddTiles
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
)
{
const
QVector
<
std
::
shared_ptr
<
const
Tile
>>
&
tileArray
,
Dispatcher
&
d
,
Rosbridge
&
rb
)
{
// qDebug() << "_callAddTiles called";
// qDebug() << "_callAddTiles called";
this
->
_lastCall
=
CALL_NAME
::
ADD_TILES
;
// create json object
// create json object
QJsonArray
jsonTileArray
;
QJsonArray
jsonTileArray
;
for
(
auto
&&
tile
:
*
pT
ileArray
)
{
for
(
auto
&&
tile
:
t
ileArray
)
{
using
namespace
ros_bridge
::
messages
;
using
namespace
ros_bridge
::
messages
;
QJsonObject
jsonTile
;
QJsonObject
jsonTile
;
if
(
!
nemo_msgs
::
tile
::
toJson
(
*
tile
,
jsonTile
))
{
if
(
!
nemo_msgs
::
tile
::
toJson
(
*
tile
,
jsonTile
))
{
...
@@ -989,13 +1080,13 @@ QVariant NemoInterface::Impl::_callAddTiles(
...
@@ -989,13 +1080,13 @@ QVariant NemoInterface::Impl::_callAddTiles(
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
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
);
promise_response
->
set_value
(
false
);
}
}
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/add_tiles"
,
responseHandler
,
req
);
rb
.
callService
(
addTilesService
,
responseHandler
,
req
);
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1007,42 +1098,30 @@ QVariant NemoInterface::Impl::_callAddTiles(
...
@@ -1007,42 +1098,30 @@ QVariant NemoInterface::Impl::_callAddTiles(
break
;
break
;
}
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
d
.
isInterruptionRequested
());
if
(
abort
)
{
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"addTiles(): Websocket not responding to request."
;
<<
"addTiles(): Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
false
;
}
}
// transaction error?
// transaction error?
if
(
!
future_response
.
get
())
{
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 success
return
QVariant
(
future
.
get
())
;
return
true
;
}
}
QVariant
bool
NemoInterface
::
Impl
::
_callRemoveTiles
(
const
IDArray
&
pIdArray
,
NemoInterface
::
Impl
::
_callRemoveTiles
(
std
::
shared_ptr
<
IDArray
>
pIdArray
)
{
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callRemoveTiles called";
// qDebug() << "_callRemoveTiles called";
this
->
_lastCall
=
CALL_NAME
::
REMOVE_TILES
;
// create json object
// create json object
QJsonArray
jsonIdArray
;
QJsonArray
jsonIdArray
;
for
(
auto
&&
id
:
*
pIdArray
)
{
for
(
auto
&&
id
:
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
append
(
id
);
jsonIdArray
.
append
(
id
);
}
// for
}
// for
...
@@ -1059,13 +1138,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
...
@@ -1059,13 +1138,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
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
);
promise_response
->
set_value
(
false
);
}
}
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/remove_tiles"
,
responseHandler
,
req
);
rb
.
callService
(
removeTilesService
,
responseHandler
,
req
);
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1077,37 +1156,23 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
...
@@ -1077,37 +1156,23 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
break
;
break
;
}
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"remove_tiles(): Websocket not responding to request."
;
<<
"remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
false
;
}
}
// transaction error?
// transaction error?
if
(
!
future_response
.
get
())
{
if
(
!
future_response
.
get
())
{
return
QVariant
(
false
)
;
return
false
;
}
}
// remove remote tiles (_remoteTiles)
return
true
;
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
());
}
}
QVariant
NemoInterface
::
Impl
::
_callClearTiles
()
{
bool
NemoInterface
::
Impl
::
_callClearTiles
(
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callClearTiles called";
this
->
_lastCall
=
CALL_NAME
::
CLEAR_TILES
;
// create response handler.
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
...
@@ -1118,8 +1183,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
...
@@ -1118,8 +1183,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/clear_tiles"
,
responseHandler
,
rb
.
callService
(
clearTilesService
,
responseHandler
,
QJsonObject
());
QJsonObject
());
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1131,40 +1195,29 @@ QVariant NemoInterface::Impl::_callClearTiles() {
...
@@ -1131,40 +1195,29 @@ QVariant NemoInterface::Impl::_callClearTiles() {
break
;
break
;
}
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"Websocket not responding to request."
;
qCWarning
(
NemoInterfaceLog
)
<<
"Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
false
;
}
}
// transaction failed?
// transaction failed?
if
(
!
future_response
.
get
())
{
if
(
!
future_response
.
get
())
{
return
QVariant
(
false
)
;
return
false
;
}
}
// clear remote tiles (_remoteTiles)
return
true
;
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
());
}
}
QVariant
ProgressArray
NemoInterface
::
Impl
::
_callGetProgress
(
const
IDArray
&
pIdArray
,
NemoInterface
::
Impl
::
_callGetProgress
(
std
::
shared_ptr
<
IDArray
>
pIdArray
)
{
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callGetProgress called";
// qDebug() << "_callGetProgress called";
this
->
_lastCall
=
CALL_NAME
::
GET_PROGRESS
;
// create json object
// create json object
QJsonArray
jsonIdArray
;
QJsonArray
jsonIdArray
;
for
(
auto
&&
id
:
*
pIdArray
)
{
for
(
auto
&&
id
:
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
append
(
id
);
jsonIdArray
.
append
(
id
);
}
// for
}
// for
...
@@ -1185,15 +1238,15 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
...
@@ -1185,15 +1238,15 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
promise_response
->
set_value
(
pArray
);
promise_response
->
set_value
(
pArray
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
getProgressService
<<
"/nemo/get_progress
error while creating ProgressArray "
<<
"
error while creating ProgressArray "
"from json."
;
"from json."
;
promise_response
->
set_value
(
nullptr
);
promise_response
->
set_value
(
nullptr
);
}
}
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_progress"
,
responseHandler
,
req
);
rb
.
callService
(
getProgressService
,
responseHandler
,
req
);
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1205,38 +1258,22 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
...
@@ -1205,38 +1258,22 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
break
;
break
;
}
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
);
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 success
return
QVariant
(
future
.
get
()
);
return
*
future_response
.
get
(
);
}
}
QVariant
NemoInterface
::
Impl
::
_callGetAllProgress
()
{
ProgressArray
NemoInterface
::
Impl
::
_callGetAllProgress
(
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callGetAllProgress called";
// qDebug() << "_callGetAllProgress called";
this
->
_lastCall
=
CALL_NAME
::
GET_ALL_PROGRESS
;
// create response handler.
// create response handler.
typedef
std
::
shared_ptr
<
ProgressArray
>
ResponseType
;
typedef
std
::
shared_ptr
<
ProgressArray
>
ResponseType
;
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
...
@@ -1251,17 +1288,16 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
...
@@ -1251,17 +1288,16 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
promise_response
->
set_value
(
pArray
);
promise_response
->
set_value
(
pArray
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
getAllProgressService
<<
"/nemo/get_all_progress
error while creating ProgressArray "
<<
"
error while creating ProgressArray "
"from json. msg: "
"from json. msg: "
<<
o
;
<<
o
;
promise_response
->
set_value
(
nullptr
);
promise_response
->
set_value
(
nullptr
);
}
}
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_all_progress"
,
responseHandler
,
rb
.
callService
(
getAllProgressService
,
responseHandler
,
QJsonObject
());
QJsonObject
());
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1273,38 +1309,22 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
...
@@ -1273,38 +1309,22 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
break
;
break
;
}
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"
all_remove_tile
s(): Websocket not responding to request."
;
<<
"
_callGetAllProgres
s(): Websocket not responding to request."
;
return
QVariant
(
false
);
return
ProgressArray
(
);
}
}
// transaction error?
// transaction error?
auto
pArray
=
future_response
.
get
();
return
*
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
());
}
}
QVariant
NemoInterface
::
Impl
::
_callGetAllTiles
()
{
QVector
<
std
::
shared_ptr
<
Tile
>>
NemoInterface
::
Impl
::
_callGetAllTiles
(
Dispatcher
&
disp
,
Rosbridge
&
rb
)
{
// qDebug() << "_callGetAllProgress called";
// qDebug() << "_callGetAllProgress called";
this
->
_lastCall
=
CALL_NAME
::
GET_ALL_TILES
;
// create response handler.
// create response handler.
typedef
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
ResponseType
;
typedef
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
ResponseType
;
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
...
@@ -1324,12 +1344,12 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
...
@@ -1324,12 +1344,12 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
pArray
->
push_back
(
tile
);
pArray
->
push_back
(
tile
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles
error while creating tile."
;
<<
getAllTilesService
<<
"
error while creating tile."
;
promise_response
->
set_value
(
nullptr
);
promise_response
->
set_value
(
nullptr
);
}
}
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles
json array does not contain objects."
;
<<
getAllTilesService
<<
"
json array does not contain objects."
;
promise_response
->
set_value
(
nullptr
);
promise_response
->
set_value
(
nullptr
);
}
}
}
}
...
@@ -1337,14 +1357,13 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
...
@@ -1337,14 +1357,13 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
promise_response
->
set_value
(
pArray
);
promise_response
->
set_value
(
pArray
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
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
);
promise_response
->
set_value
(
nullptr
);
}
}
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_all_tiles"
,
responseHandler
,
rb
.
callService
(
getAllTilesService
,
responseHandler
,
QJsonObject
());
QJsonObject
());
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1356,35 +1375,18 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
...
@@ -1356,35 +1375,18 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
break
;
break
;
}
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
disp
.
isInterruptionRequested
());
if
(
abort
)
{
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QV
ariant
(
false
);
return
QV
ector
<
std
::
shared_ptr
<
Tile
>>
(
);
}
}
// transaction error?
return
*
future_response
.
get
();
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
());
}
}
QVariant
NemoInterface
::
Impl
::
_callGetVersion
()
{
QString
NemoInterface
::
Impl
::
_callGetVersion
(
Dispatcher
&
d
,
Rosbridge
&
rb
)
{
this
->
_lastCall
=
CALL_NAME
::
GET_VERSION
;
// create response handler.
// create response handler.
typedef
QString
ResponseType
;
typedef
QString
ResponseType
;
...
@@ -1399,14 +1401,13 @@ QVariant NemoInterface::Impl::_callGetVersion() {
...
@@ -1399,14 +1401,13 @@ QVariant NemoInterface::Impl::_callGetVersion() {
promise_response
->
set_value
(
version
);
promise_response
->
set_value
(
version
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_version
no version key or wrong type."
;
<<
getVersionService
<<
"
no version key or wrong type."
;
promise_response
->
set_value
(
"error!"
);
promise_response
->
set_value
(
"error!"
);
}
}
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_version"
,
responseHandler
,
rb
.
callService
(
getVersionService
,
responseHandler
,
QJsonObject
());
QJsonObject
());
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1418,148 +1419,74 @@ QVariant NemoInterface::Impl::_callGetVersion() {
...
@@ -1418,148 +1419,74 @@ QVariant NemoInterface::Impl::_callGetVersion() {
break
;
break
;
}
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
d
.
isInterruptionRequested
());
if
(
abort
)
{
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
)
;
return
"unknown_version"
;
}
}
// transaction error?
// transaction error?
auto
version
=
future_response
.
get
();
auto
version
=
future_response
.
get
();
if
(
version
==
"error!"
)
{
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 success
return
QVariant
(
future
.
get
());
return
version
;
}
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"
);
}
}
void
NemoInterface
::
Impl
::
_addTilesRemote
(
bool
NemoInterface
::
Impl
::
_addTilesRemote
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
pTileArray
,
const
QVector
<
std
::
shared_ptr
<
const
Tile
>>
tileArray
)
{
std
::
promise
<
bool
>
promise
)
{
// qDebug() << "_addTilesRemote called";
// qDebug() << "_addTilesRemote called";
auto
pArrayDup
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
()
;
QVector
<
std
::
shared_ptr
<
Tile
>>
copy
;
for
(
auto
&&
pTile
:
*
pT
ileArray
)
{
for
(
auto
&&
pTile
:
t
ileArray
)
{
pArrayDup
->
push_back
(
std
::
make_shared
<
Tile
>
(
*
pTile
));
copy
.
push_back
(
std
::
make_shared
<
Tile
>
(
*
pTile
));
}
}
_addTilesRemote2
(
pArrayDup
,
std
::
move
(
promise
)
);
return
_addTilesRemote
(
copy
);
}
}
void
NemoInterface
::
Impl
::
_addTilesRemote2
(
bool
NemoInterface
::
Impl
::
_addTilesRemote
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
pTileArray
,
const
QVector
<
std
::
shared_ptr
<
Tile
>>
&
tileArray
)
{
std
::
promise
<
bool
>
promise
)
{
// qDebug() << "_addTilesRemote2 called";
bool
error
=
false
;
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
id
=
pTile
->
id
();
auto
it
=
_remoteTiles
.
find
(
id
);
auto
it
=
_remoteTiles
.
find
(
id
);
if
(
Q_LIKELY
(
it
==
_remoteTiles
.
end
()))
{
if
(
Q_LIKELY
(
it
==
_remoteTiles
.
end
()))
{
auto
ret
=
_remoteTiles
.
insert
(
std
::
make_pair
(
id
,
pTile
));
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_ASSERT
(
ret
.
second
==
true
);
Q_UNUSED
(
ret
);
Q_UNUSED
(
ret
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"_addTilesRemote: tile with id "
<<
id
<<
" already added."
;
if
(
pTile
->
tile
()
!=
it
->
second
->
tile
())
{
if
(
pTile
->
tile
()
!=
it
->
second
->
tile
())
{
qCWarning
(
NemoInterfaceLog
)
<<
"_addTilesRemote: tiles differ but have the same id."
;
error
=
true
;
error
=
true
;
}
}
}
}
}
}
if
(
array
.
size
()
>
0
)
{
lk
.
unlock
();
if
(
this
->
_isSynchronized
())
{
if
(
anyChange
>
0
)
{
this
->
_setState
(
STATE
::
READY
);
emit
this
->
_parent
->
tilesChanged
();
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
>
());
}
}
promise
.
set_value
(
true
);
return
!
error
;
}
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
);
}
}
void
NemoInterface
::
Impl
::
_removeTilesRemote
(
std
::
shared_ptr
<
IDArray
>
idArray
,
void
NemoInterface
::
Impl
::
_removeTilesRemote
(
const
IDArray
&
idArray
)
{
std
::
promise
<
bool
>
promise
)
{
// qDebug() << "_removeTilesRemote called";
// qDebug() << "_removeTilesRemote called";
bool
anyChange
=
false
;
bool
anyChange
=
false
;
for
(
auto
&&
id
:
*
idArray
)
{
Lock
lk
(
this
->
_m
);
for
(
auto
&&
id
:
idArray
)
{
auto
it
=
_remoteTiles
.
find
(
id
);
auto
it
=
_remoteTiles
.
find
(
id
);
if
(
Q_LIKELY
(
it
!=
_remoteTiles
.
end
()))
{
if
(
Q_LIKELY
(
it
!=
_remoteTiles
.
end
()))
{
_remoteTiles
.
erase
(
it
);
_remoteTiles
.
erase
(
it
);
...
@@ -1571,30 +1498,25 @@ void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr<IDArray> idArray,
...
@@ -1571,30 +1498,25 @@ void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr<IDArray> idArray,
}
}
if
(
anyChange
)
{
if
(
anyChange
)
{
if
(
this
->
_isSynchronized
())
{
emit
this
->
_parent
->
tilesChanged
();
this
->
_setState
(
STATE
::
READY
);
this
->
_doAction
();
}
}
}
}
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";
// qDebug() << "_clearTilesRemote called";
if
(
_remoteTiles
.
size
()
>
0
)
{
if
(
_remoteTiles
.
size
()
>
0
)
{
_remoteTiles
.
clear
();
_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
)
{
if
(
newState
!=
this
->
_state
)
{
auto
oldState
=
this
->
_state
.
load
()
;
auto
oldState
=
this
->
_state
;
this
->
_state
=
newState
;
this
->
_state
=
newState
;
qCDebug
(
NemoInterfaceLog
)
qCDebug
(
NemoInterfaceLog
)
...
@@ -1602,11 +1524,15 @@ bool NemoInterface::Impl::_setState(STATE newState) {
...
@@ -1602,11 +1524,15 @@ bool NemoInterface::Impl::_setState(STATE newState) {
auto
oldStatus
=
_status
(
oldState
);
auto
oldStatus
=
_status
(
oldState
);
auto
newStatus
=
_status
(
newState
);
auto
newStatus
=
_status
(
newState
);
if
(
oldStatus
!=
newStatus
)
{
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
))
{
if
(
_runningNotSafe
(
oldState
)
!=
_runningNotSafe
(
newState
))
{
emit
this
->
_parent
->
runningChanged
();
// emit signal later
QTimer
::
singleShot
(
5
,
this
->
_parent
,
[
this
]
{
emit
this
->
_parent
->
runningChanged
();
});
}
}
return
true
;
return
true
;
...
@@ -1615,19 +1541,19 @@ bool NemoInterface::Impl::_setState(STATE newState) {
...
@@ -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
;
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
;
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
;
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
;
return
s
!=
STATE
::
STOPPED
;
}
}
...
@@ -1654,6 +1580,9 @@ NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) {
...
@@ -1654,6 +1580,9 @@ NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) {
case
STATE
:
:
SYS_SYNC
:
case
STATE
:
:
SYS_SYNC
:
status
=
NemoInterface
::
STATUS
::
SYNC
;
status
=
NemoInterface
::
STATUS
::
SYNC
;
break
;
break
;
case
STATE
:
:
SYNC_ERROR
:
status
=
NemoInterface
::
STATUS
::
ERROR
;
break
;
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
status
=
NemoInterface
::
STATUS
::
TIMEOUT
;
status
=
NemoInterface
::
STATUS
::
TIMEOUT
;
...
@@ -1672,13 +1601,15 @@ QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) {
...
@@ -1672,13 +1601,15 @@ QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) {
case
STATE
:
:
WEBSOCKET_DETECTED
:
case
STATE
:
:
WEBSOCKET_DETECTED
:
return
QString
(
"WEBSOCKET_DETECTED"
);
return
QString
(
"WEBSOCKET_DETECTED"
);
case
STATE
:
:
TRY_SETUP
:
case
STATE
:
:
TRY_SETUP
:
return
QString
(
"TRY_
TOPIC_SERVICE_
SETUP"
);
return
QString
(
"TRY_SETUP"
);
case
STATE
:
:
READY
:
case
STATE
:
:
READY
:
return
QString
(
"READY"
);
return
QString
(
"READY"
);
case
STATE
:
:
USER_SYNC
:
case
STATE
:
:
USER_SYNC
:
return
QString
(
"SYNC_USER"
);
return
QString
(
"SYNC_USER"
);
case
STATE
:
:
SYS_SYNC
:
case
STATE
:
:
SYS_SYNC
:
return
QString
(
"SYNC_SYS"
);
return
QString
(
"SYNC_SYS"
);
case
STATE
:
:
SYNC_ERROR
:
return
QString
(
"SYNC_ERROR"
);
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
return
QString
(
"WEBSOCKET_TIMEOUT"
);
return
QString
(
"WEBSOCKET_TIMEOUT"
);
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
...
@@ -1705,6 +1636,98 @@ QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) {
...
@@ -1705,6 +1636,98 @@ QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) {
return
"unknown state!"
;
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
::
NemoInterface
()
NemoInterface
::
NemoInterface
()
...
...
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp
View file @
7308bf55
...
@@ -52,7 +52,11 @@ void TaskDispatcher::start() {
...
@@ -52,7 +52,11 @@ void TaskDispatcher::start() {
ULock
lk1
(
this
->
_mutex
);
ULock
lk1
(
this
->
_mutex
);
if
(
!
_running
)
{
if
(
!
_running
)
{
this
->
_running
=
true
;
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
();
lk1
.
unlock
();
}
}
}
}
...
@@ -61,8 +65,8 @@ void TaskDispatcher::stop() {
...
@@ -61,8 +65,8 @@ void TaskDispatcher::stop() {
ULock
lk1
(
this
->
_mutex
);
ULock
lk1
(
this
->
_mutex
);
if
(
_running
)
{
if
(
_running
)
{
this
->
_running
=
false
;
this
->
_running
=
false
;
_threadFuture
.
wait
();
lk1
.
unlock
();
lk1
.
unlock
();
this
->
_future
.
waitForFinished
();
}
}
}
}
...
@@ -88,7 +92,7 @@ std::size_t TaskDispatcher::pendingTasks() {
...
@@ -88,7 +92,7 @@ std::size_t TaskDispatcher::pendingTasks() {
bool
TaskDispatcher
::
idle
()
{
return
this
->
pendingTasks
()
==
0
;
}
bool
TaskDispatcher
::
idle
()
{
return
this
->
pendingTasks
()
==
0
;
}
void
TaskDispatcher
::
run
()
{
void
TaskDispatcher
::
run
(
std
::
promise
<
void
>
p
)
{
while
(
true
)
{
while
(
true
)
{
ULock
lk1
(
this
->
_mutex
);
ULock
lk1
(
this
->
_mutex
);
...
@@ -112,6 +116,7 @@ void TaskDispatcher::run() {
...
@@ -112,6 +116,7 @@ void TaskDispatcher::run() {
break
;
break
;
}
}
}
}
p
.
set_value
();
}
}
}
// namespace nemo_interface
}
// namespace nemo_interface
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h
View file @
7308bf55
...
@@ -56,13 +56,12 @@ public:
...
@@ -56,13 +56,12 @@ public:
bool
idle
();
bool
idle
();
protected:
protected:
void
run
();
void
run
(
std
::
promise
<
void
>
p
);
private:
private:
QFuture
<
void
>
_future
;
std
::
deque
<
std
::
unique_ptr
<
Task
>>
_taskQueue
;
std
::
deque
<
std
::
unique_ptr
<
Task
>>
_taskQueue
;
std
::
deque
<
std
::
promise
<
QVariant
>>
_promiseQueue
;
std
::
deque
<
std
::
promise
<
QVariant
>>
_promiseQueue
;
std
::
future
<
void
>
_threadFuture
;
bool
_running
;
bool
_running
;
std
::
mutex
_mutex
;
std
::
mutex
_mutex
;
};
};
...
...
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
View file @
7308bf55
...
@@ -37,7 +37,7 @@ void Rosbridge::start() {
...
@@ -37,7 +37,7 @@ void Rosbridge::start() {
_impl
->
moveToThread
(
_t
);
_impl
->
moveToThread
(
_t
);
connect
(
_impl
,
&
RosbridgeImpl
::
stateChanged
,
this
,
connect
(
_impl
,
&
RosbridgeImpl
::
stateChanged
,
this
,
&
Rosbridge
::
s
tateChanged
);
&
Rosbridge
::
_onImplS
tateChanged
);
connect
(
this
,
&
Rosbridge
::
_start
,
_impl
,
&
RosbridgeImpl
::
start
);
connect
(
this
,
&
Rosbridge
::
_start
,
_impl
,
&
RosbridgeImpl
::
start
);
connect
(
this
,
&
Rosbridge
::
_stop
,
_impl
,
&
RosbridgeImpl
::
stop
);
connect
(
this
,
&
Rosbridge
::
_stop
,
_impl
,
&
RosbridgeImpl
::
stop
);
...
@@ -173,6 +173,26 @@ void Rosbridge::waitForTopic(const QString &topic) {
...
@@ -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
,
void
Rosbridge
::
advertiseService
(
const
QString
&
service
,
const
QString
&
type
,
CallBackWReturn
callback
)
{
CallBackWReturn
callback
)
{
emit
_advertiseService
(
service
,
type
,
callback
);
emit
_advertiseService
(
service
,
type
,
callback
);
...
@@ -244,6 +264,26 @@ void Rosbridge::waitForService(const QString &service) {
...
@@ -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
)
{
Rosbridge
::
STATE
translate
(
RosbridgeImpl
::
STATE
s
)
{
switch
(
s
)
{
switch
(
s
)
{
case
RosbridgeImpl
:
:
STATE
::
STOPPED
:
case
RosbridgeImpl
:
:
STATE
::
STOPPED
:
...
@@ -260,3 +300,4 @@ Rosbridge::STATE translate(RosbridgeImpl::STATE s) {
...
@@ -260,3 +300,4 @@ Rosbridge::STATE translate(RosbridgeImpl::STATE s) {
qCritical
()
<<
"unreachable branch!"
;
qCritical
()
<<
"unreachable branch!"
;
return
Rosbridge
::
STATE
::
STOPPED
;
return
Rosbridge
::
STATE
::
STOPPED
;
}
}
void
Rosbridge
::
_onImplStateChanged
()
{
emit
stateChanged
();
}
src/MeasurementComplexItem/rosbridge/rosbridge.h
View file @
7308bf55
...
@@ -58,6 +58,8 @@ public:
...
@@ -58,6 +58,8 @@ public:
void
unsubscribeAllTopics
();
void
unsubscribeAllTopics
();
void
waitForTopic
(
const
QString
&
topic
);
void
waitForTopic
(
const
QString
&
topic
);
bool
waitForTopic
(
const
QString
&
topic
,
const
std
::
function
<
bool
()
>
&
condition
);
// Services.
// Services.
void
advertiseService
(
const
QString
&
service
,
const
QString
&
type
,
void
advertiseService
(
const
QString
&
service
,
const
QString
&
type
,
...
@@ -75,6 +77,8 @@ public:
...
@@ -75,6 +77,8 @@ public:
std
::
future
<
QJsonObject
>
getAdvertisedServices
();
std
::
future
<
QJsonObject
>
getAdvertisedServices
();
void
waitForService
(
const
QString
&
service
);
void
waitForService
(
const
QString
&
service
);
bool
waitForService
(
const
QString
&
service
,
const
std
::
function
<
bool
()
>
&
condition
);
signals:
signals:
void
stateChanged
();
void
stateChanged
();
...
@@ -99,6 +103,8 @@ signals:
...
@@ -99,6 +103,8 @@ signals:
const
QJsonObject
&
args
=
QJsonObject
());
const
QJsonObject
&
args
=
QJsonObject
());
void
_unadvertiseService
(
const
QString
&
service
);
void
_unadvertiseService
(
const
QString
&
service
);
void
_unadvertiseAllServices
();
void
_unadvertiseAllServices
();
private
slots
:
void
_onImplStateChanged
();
private:
private:
std
::
atomic
<
STATE
>
_state
;
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