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
7a0b738b
Commit
7a0b738b
authored
Jan 26, 2021
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
version checking added to nemo inteface, testing needed..
parent
b098b99a
Changes
12
Show whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
596 additions
and
111 deletions
+596
-111
qgroundcontrol.pro
qgroundcontrol.pro
+2
-0
NemoInterface.cpp
src/MeasurementComplexItem/NemoInterface.cpp
+510
-93
NemoInterface.h
src/MeasurementComplexItem/NemoInterface.h
+1
-0
MeasurementTile.cpp
...MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
+10
-1
NemoEditor.qml
src/MeasurementComplexItem/qml/NemoEditor.qml
+9
-2
rosbridge.cpp
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
+1
-11
rosbridge.h
src/MeasurementComplexItem/rosbridge/rosbridge.h
+0
-2
rosbridgeimpl.cpp
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp
+5
-1
rosbridgeimpl.h
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h
+1
-0
tile.h
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
+1
-1
tile_array.cpp
...comm/ros_bridge/include/messages/nemo_msgs/tile_array.cpp
+1
-0
tile_array.h
src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.h
+55
-0
No files found.
qgroundcontrol.pro
View file @
7a0b738b
...
...
@@ -517,6 +517,7 @@ HEADERS += \
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile_array
.
h
\
src
/
comm
/
utilities
.
h
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
...
...
@@ -559,6 +560,7 @@ SOURCES += \
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
cpp
\
src
/
Settings
/
WimaSettings
.
cc
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile_array
.
cpp
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
SOURCES
+=
\
...
...
src/MeasurementComplexItem/NemoInterface.cpp
View file @
7a0b738b
...
...
@@ -26,12 +26,16 @@
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/progress_array.h"
#include "ros_bridge/include/messages/nemo_msgs/tile.h"
#include "ros_bridge/include/messages/nemo_msgs/tile_array.h"
#include "rosbridge/rosbridge.h"
QGC_LOGGING_CATEGORY
(
NemoInterfaceLog
,
"NemoInterfaceLog"
)
#define SYNC_INTERVAL 1000 // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
#define RESTART_INTERVAl 600000 // ms == 10 min
#define RESTART_RETRY_INTERVAl 2000 // ms
#define SYNC_INTERVAL 10000 // ms
#define SYNC_RETRY_INTERVAL 2000 // ms
static
constexpr
auto
maxResponseTime
=
std
::
chrono
::
milliseconds
(
10000
);
static
const
char
*
progressTopic
=
"/nemo/progress"
;
...
...
@@ -55,7 +59,7 @@ class NemoInterface::Impl {
STOPPED
,
START_BRIDGE
,
WEBSOCKET_DETECTED
,
TRY_
TOPIC_SERVICE_
SETUP
,
TRY_SETUP
,
USER_SYNC
,
SYS_SYNC
,
READY
,
...
...
@@ -96,8 +100,13 @@ public:
private:
void
_doTopicServiceSetup
();
void
_checkVersion
();
void
_subscribeProgressTopic
();
void
_subscribeHearbeatTopic
();
void
_doAction
();
void
_trySynchronize
();
void
_synchronizeIfNeccessary
();
void
_tryRestart
();
bool
_isSynchronized
()
const
;
bool
_userSync
()
const
;
// thread safe
bool
_sysSync
()
const
;
// thread safe
...
...
@@ -115,12 +124,16 @@ private:
// 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_PROGRESS
GET_ALL_TILES
,
GET_ALL_PROGRESS
,
GET_VERSION
};
QString
_toString
(
CALL_NAME
name
);
...
...
@@ -130,6 +143,9 @@ private:
void
_addTilesRemote2
(
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
pTileArray
,
std
::
promise
<
bool
>
promise
);
void
_setTiles
(
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
);
...
...
@@ -141,6 +157,7 @@ private:
void
_setWarningString
(
const
QString
&
warning
);
bool
_setState
(
STATE
newState
);
// not thread safe
static
bool
_ready
(
STATE
s
);
static
bool
_userSync
(
STATE
s
);
static
bool
_sysSync
(
STATE
s
);
...
...
@@ -149,7 +166,13 @@ private:
static
QString
_toString
(
STATE
s
);
static
QString
_toString
(
NemoInterface
::
STATUS
s
);
static
QString
_localVersion
;
QString
_remoteVersion
;
std
::
atomic
<
STATE
>
_state
;
std
::
atomic_bool
_versionOK
;
std
::
atomic_bool
_progressTopicOK
;
std
::
atomic_bool
_heartbeatTopicOK
;
std
::
atomic
<
CALL_NAME
>
_lastCall
;
ROSBridgePtr
_pRosbridge
;
TileMap
_remoteTiles
;
...
...
@@ -159,14 +182,20 @@ private:
QString
_infoString
;
QString
_warningString
;
QTimer
_timeoutTimer
;
QTimer
_syncTimer
;
QTimer
_restartTimer
;
QNemoHeartbeat
_lastHeartbeat
;
FutureWatcher
_futureWatcher
;
};
QString
NemoInterface
::
Impl
::
_localVersion
(
"V_1.0"
);
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
static
StatusMap
statusMap
{
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
NOT_CONNECTED
,
"Not Connected"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
ERROR
,
"ERROR"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
SYNC
,
"Synchronizing"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
READY
,
...
...
@@ -177,7 +206,8 @@ static StatusMap statusMap{
NemoInterface
::
STATUS
::
WEBSOCKET_DETECTED
,
"Websocket Detected"
)};
NemoInterface
::
Impl
::
Impl
(
NemoInterface
*
p
)
:
_state
(
STATE
::
STOPPED
),
_parent
(
p
)
{
:
_state
(
STATE
::
STOPPED
),
_versionOK
(
false
),
_progressTopicOK
(
false
),
_heartbeatTopicOK
(
false
),
_parent
(
p
)
{
// ROS Bridge.
WimaSettings
*
wimaSettings
=
...
...
@@ -202,12 +232,17 @@ NemoInterface::Impl::Impl(NemoInterface *p)
connect
(
&
this
->
_timeoutTimer
,
&
QTimer
::
timeout
,
std
::
bind
(
&
Impl
::
_onHeartbeatTimeout
,
this
));
// Connection timer (temporary workaround)
connect
(
this
->
_pRosbridge
.
get
(),
&
Rosbridge
::
stateChanged
,
connect
(
this
->
_pRosbridge
.
get
(),
&
Rosbridge
::
stateChanged
,
this
->
_parent
,
[
this
]
{
this
->
_onRosbridgeStateChanged
();
});
connect
(
&
this
->
_futureWatcher
,
&
FutureWatcher
::
finished
,
connect
(
&
this
->
_futureWatcher
,
&
FutureWatcher
::
finished
,
this
->
_parent
,
[
this
]
{
this
->
_onFutureWatcherFinished
();
});
connect
(
&
this
->
_restartTimer
,
&
QTimer
::
timeout
,
this
->
_parent
,
[
this
]
{
this
->
_tryRestart
();
});
connect
(
&
this
->
_syncTimer
,
&
QTimer
::
timeout
,
this
->
_parent
,
[
this
]
{
this
->
_synchronizeIfNeccessary
();
});
}
NemoInterface
::
Impl
::~
Impl
()
{
this
->
_pRosbridge
->
stop
();
}
...
...
@@ -253,7 +288,7 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
}
}
if
(
pTileArray
->
size
()
>
0
)
{
this
->
_parent
->
tilesChanged
();
emit
this
->
_parent
->
tilesChanged
();
}
// ready for send?
...
...
@@ -303,7 +338,7 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
// copy known ids
auto
pIdArray
=
std
::
make_shared
<
IDArray
>
();
for
(
const
auto
id
:
idArray
)
{
for
(
const
auto
&
id
:
idArray
)
{
const
auto
it
=
this
->
_localTiles
.
find
(
id
);
Q_ASSERT
(
it
!=
_localTiles
.
end
());
if
(
Q_LIKELY
(
it
!=
_localTiles
.
end
()))
{
...
...
@@ -314,7 +349,7 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
}
}
if
(
pIdArray
->
size
()
>
0
)
{
this
->
_parent
->
tilesChanged
();
emit
this
->
_parent
->
tilesChanged
();
}
// ready for send?
...
...
@@ -348,7 +383,7 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
// clear local tiles (_localTiles)
if
(
!
_localTiles
.
empty
())
{
this
->
_localTiles
.
clear
();
this
->
_parent
->
tilesChanged
();
emit
this
->
_parent
->
tilesChanged
();
}
if
(
this
->
ready
()
||
this
->
_userSync
())
{
...
...
@@ -498,11 +533,23 @@ bool NemoInterface::Impl::_sysSync() const { return _sysSync(this->_state); }
void
NemoInterface
::
Impl
::
_onFutureWatcherFinished
()
{
if
(
this
->
ready
()
||
this
->
_userSync
()
||
this
->
_sysSync
())
{
static
long
tries
=
0
;
auto
lastTransactionSuccessfull
=
_futureWatcher
.
result
().
toBool
();
if
(
!
lastTransactionSuccessfull
)
{
++
tries
;
qCDebug
(
NemoInterfaceLog
)
<<
"last transaction unsuccessfull: "
<<
_toString
(
_lastCall
);
QTimer
::
singleShot
(
5000
,
[
this
]
{
this
->
_trySynchronize
();
});
if
(
tries
<
5
)
{
QTimer
::
singleShot
(
SYNC_RETRY_INTERVAL
,
this
->
_parent
,
[
this
]
{
this
->
_trySynchronize
();
});
}
else
{
_setWarningString
(
"The last 5 transactions failed! Please check the "
"connection and consider reseting the connection."
);
tries
=
0
;
}
}
else
{
tries
=
0
;
}
}
}
...
...
@@ -521,8 +568,7 @@ void NemoInterface::Impl::_onRosbridgeStateChanged() {
this
->
_doAction
();
}
}
else
if
(
state
==
Rosbridge
::
STATE
::
TIMEOUT
)
{
if
(
this
->
_state
==
STATE
::
TRY_TOPIC_SERVICE_SETUP
||
this
->
_state
==
STATE
::
READY
||
if
(
this
->
_state
==
STATE
::
TRY_SETUP
||
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
WEBSOCKET_DETECTED
||
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_TIMEOUT
);
...
...
@@ -570,7 +616,7 @@ void NemoInterface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &hb,
std
::
promise
<
bool
>
promise
)
{
_lastHeartbeat
=
hb
;
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
if
(
this
->
_state
==
STATE
::
TRY_
TOPIC_SERVICE_
SETUP
)
{
if
(
this
->
_state
==
STATE
::
TRY_SETUP
)
{
this
->
_setState
(
STATE
::
READY
);
this
->
_doAction
();
}
else
if
(
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
...
...
@@ -594,14 +640,87 @@ void NemoInterface::Impl::_setWarningString(const QString &warning) {
}
}
void
NemoInterface
::
Impl
::
_doTopicServiceSetup
()
{
using
namespace
ros_bridge
::
messages
;
void
NemoInterface
::
Impl
::
_doTopicServiceSetup
()
{}
// Subscribe nemo progress.
this
->
_pRosbridge
->
subscribeTopic
(
progressTopic
,
[
this
](
const
QJsonObject
&
o
)
{
nemo_msgs
::
progress_array
::
ProgressArray
progressArray
;
if
(
nemo_msgs
::
progress_array
::
fromJson
(
o
,
progressArray
))
{
void
NemoInterface
::
Impl
::
_checkVersion
()
{
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
]
{
// wait for service
std
::
future
<
bool
>
fut
;
long
tries
=
0
;
long
maxTries
=
50
;
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
;
}
}
++
tries
;
if
(
tries
>
maxTries
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"_checkVersion: service /nemo/get_version not available."
;
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
/* context */
,
[
this
]()
{
this
->
_setWarningString
(
"Version checking failed."
);
});
Q_ASSERT
(
value
==
true
);
return
QVariant
(
false
);
}
}
while
(
!
fut
.
get
());
// call service
return
this
->
_callGetVersion
();
});
this
->
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
}
void
NemoInterface
::
Impl
::
_subscribeProgressTopic
()
{
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
([
this
]
{
// wait for service
std
::
future
<
bool
>
fut
;
long
tries
=
0
;
long
maxTries
=
50
;
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
;
}
}
++
tries
;
if
(
tries
>
maxTries
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"_subscribeProgressTopic: topic /nemo/progress not available."
;
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
/* context */
,
[
this
]()
{
this
->
_setWarningString
(
"Progress topic not available."
);
});
Q_ASSERT
(
value
==
true
);
return
QVariant
(
false
);
}
}
while
(
!
fut
.
get
());
// subscribe
this
->
_pRosbridge
->
subscribeTopic
(
progressTopic
,
[
this
](
const
QJsonObject
&
o
)
{
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
progressArray
;
if
(
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
fromJson
(
o
,
progressArray
))
{
// correct range errors of progress
for
(
auto
&
lp
:
progressArray
.
progress_array
())
{
...
...
@@ -638,8 +757,51 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
<<
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
;
long
tries
=
0
;
long
maxTries
=
50
;
do
{
fut
=
this
->
_pRosbridge
->
topicAvailable
(
heartbeatTopic
);
// Subscribe heartbeat msg.
// 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
;
}
}
++
tries
;
if
(
tries
>
maxTries
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"_subscribeHeartbeatTopic: topic /nemo/hearbeat not available."
;
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
/* context */
,
[
this
]()
{
this
->
_setWarningString
(
"Heartbeat topic not available."
);
});
Q_ASSERT
(
value
==
true
);
return
QVariant
(
false
);
}
}
while
(
!
fut
.
get
());
// subscribe
using
namespace
ros_bridge
::
messages
;
this
->
_pRosbridge
->
subscribeTopic
(
heartbeatTopic
,
[
this
](
const
QJsonObject
&
o
)
{
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
...
...
@@ -659,6 +821,16 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
<<
o
;
}
});
this
->
_heartbeatTopicOK
=
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
::
_trySynchronize
()
{
...
...
@@ -667,7 +839,9 @@ void NemoInterface::Impl::_trySynchronize() {
!
_isSynchronized
())
{
if
(
!
_dispatcher
.
idle
())
{
QTimer
::
singleShot
(
5000
,
[
this
]
{
this
->
_trySynchronize
();
});
QTimer
::
singleShot
(
SYNC_RETRY_INTERVAL
,
this
->
_parent
,
[
this
]
{
this
->
_trySynchronize
();
});
qCWarning
(
NemoInterfaceLog
)
<<
"synchronization defered"
;
return
;
}
...
...
@@ -687,7 +861,7 @@ void NemoInterface::Impl::_trySynchronize() {
// create tile array.
auto
pTileArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
const
Tile
>>>
();
for
(
auto
pair
:
_localTiles
)
{
for
(
const
auto
&
pair
:
_localTiles
)
{
pTileArray
->
push_back
(
pair
.
second
);
}
...
...
@@ -722,6 +896,35 @@ void NemoInterface::Impl::_trySynchronize() {
}
}
void
NemoInterface
::
Impl
::
_synchronizeIfNeccessary
()
{
if
(
_dispatcher
.
idle
())
{
// create getAllTiles cmd.
auto
pTask
=
std
::
make_unique
<
nemo_interface
::
Task
>
(
[
this
]
{
return
this
->
_callGetAllTiles
();
});
// dispatch command.
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
fut
=
ret
.
share
();
_futureWatcher
.
setFuture
(
fut
);
_syncTimer
.
start
(
SYNC_INTERVAL
);
}
else
{
_syncTimer
.
start
(
SYNC_RETRY_INTERVAL
);
}
}
void
NemoInterface
::
Impl
::
_tryRestart
()
{
if
(
this
->
running
())
{
if
(
_dispatcher
.
idle
())
{
qDebug
()
<<
"_tryRestart: restarting"
;
this
->
stop
();
this
->
start
();
_restartTimer
.
start
(
RESTART_INTERVAl
);
}
else
{
_restartTimer
.
start
(
RESTART_RETRY_INTERVAl
);
}
}
}
bool
NemoInterface
::
Impl
::
_isSynchronized
()
const
{
return
_localTiles
.
size
()
>
0
&&
_remoteTiles
.
size
()
>
0
&&
std
::
equal
(
...
...
@@ -733,33 +936,50 @@ void NemoInterface::Impl::_doAction() {
static
bool
resetDone
=
false
;
switch
(
this
->
_state
)
{
case
STATE
:
:
STOPPED
:
_setWarningString
(
""
);
_setInfoString
(
""
);
this
->
_timeoutTimer
.
stop
();
this
->
_restartTimer
.
stop
();
this
->
_syncTimer
.
stop
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
if
(
this
->
_pRosbridge
->
state
()
!=
Rosbridge
::
STATE
::
STOPPED
)
{
this
->
_pRosbridge
->
stop
();
}
_versionOK
=
false
;
_progressTopicOK
=
false
;
_heartbeatTopicOK
=
false
;
break
;
case
STATE
:
:
START_BRIDGE
:
this
->
_pRosbridge
->
start
();
this
->
_restartTimer
.
start
(
RESTART_INTERVAl
);
break
;
case
STATE
:
:
WEBSOCKET_DETECTED
:
resetDone
=
false
;
this
->
_setState
(
STATE
::
TRY_
TOPIC_SERVICE_
SETUP
);
this
->
_setState
(
STATE
::
TRY_SETUP
);
this
->
_doAction
();
break
;
case
STATE
:
:
TRY_TOPIC_SERVICE_SETUP
:
this
->
_doTopicServiceSetup
();
case
STATE
:
:
TRY_SETUP
:
if
(
!
_versionOK
)
{
this
->
_checkVersion
();
}
else
if
(
!
_progressTopicOK
)
{
this
->
_subscribeProgressTopic
();
}
else
if
(
!
_heartbeatTopicOK
)
{
this
->
_subscribeHearbeatTopic
();
}
else
{
this
->
_timeoutTimer
.
start
(
NO_HEARTBEAT_TIMEOUT
);
}
break
;
case
STATE
:
:
READY
:
_trySynchronize
();
this
->
_trySynchronize
();
this
->
_syncTimer
.
start
(
SYNC_INTERVAL
);
break
;
case
STATE
:
:
USER_SYNC
:
case
STATE
:
:
SYS_SYNC
:
break
;
case
STATE
:
:
HEARTBEAT_TIMEOUT
:
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
this
->
_syncTimer
.
stop
();
break
;
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
if
(
!
resetDone
)
{
...
...
@@ -768,7 +988,11 @@ void NemoInterface::Impl::_doAction() {
this
->
_pRosbridge
->
start
();
}
this
->
_timeoutTimer
.
stop
();
this
->
_syncTimer
.
stop
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
_versionOK
=
false
;
_progressTopicOK
=
false
;
_heartbeatTopicOK
=
false
;
break
;
};
}
...
...
@@ -782,7 +1006,7 @@ QVariant NemoInterface::Impl::_callAddTiles(
// create json object
QJsonArray
jsonTileArray
;
for
(
const
auto
&
tile
:
*
pTileArray
)
{
for
(
auto
&
&
tile
:
*
pTileArray
)
{
using
namespace
ros_bridge
::
messages
;
QJsonObject
jsonTile
;
if
(
!
nemo_msgs
::
tile
::
toJson
(
*
tile
,
jsonTile
))
{
...
...
@@ -859,7 +1083,7 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
// create json object
QJsonArray
jsonIdArray
;
for
(
const
auto
id
:
*
pIdArray
)
{
for
(
auto
&&
id
:
*
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
append
(
id
);
}
// for
...
...
@@ -929,7 +1153,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
)
mutable
{
// check if transaction was successfull
promise_response
->
set_value
(
true
);
};
...
...
@@ -981,7 +1205,7 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
// create json object
QJsonArray
jsonIdArray
;
for
(
const
auto
id
:
*
pIdArray
)
{
for
(
auto
&&
id
:
*
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
append
(
id
);
}
// for
...
...
@@ -1117,9 +1341,153 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
return
QVariant
(
future
.
get
());
}
QVariant
NemoInterface
::
Impl
::
_callGetAllTiles
()
{
// qDebug() << "_callGetAllProgress called";
this
->
_lastCall
=
CALL_NAME
::
GET_ALL_TILES
;
// create response handler.
typedef
std
::
shared_ptr
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
ResponseType
;
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
const
char
*
const
tileArrayKey
=
"tile_array"
;
// check if transaction was successfull
if
(
o
.
contains
(
tileArrayKey
)
&&
o
[
tileArrayKey
].
isArray
())
{
auto
pArray
=
std
::
make_shared
<
QVector
<
std
::
shared_ptr
<
Tile
>>>
();
const
auto
jsonArray
=
o
[
tileArrayKey
].
toArray
();
for
(
int
i
=
0
;
i
<
jsonArray
.
size
();
++
i
)
{
if
(
jsonArray
[
i
].
isObject
())
{
QJsonObject
o
=
jsonArray
[
i
].
toObject
();
auto
tile
=
std
::
make_shared
<
Tile
>
();
if
(
ros_bridge
::
messages
::
nemo_msgs
::
tile
::
fromJson
(
o
,
*
tile
))
{
pArray
->
push_back
(
tile
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles error while creating tile."
;
promise_response
->
set_value
(
nullptr
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles json array does not contain objects."
;
promise_response
->
set_value
(
nullptr
);
}
}
// success!
promise_response
->
set_value
(
pArray
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_all_tiles no tile_array key or wrong type."
;
promise_response
->
set_value
(
nullptr
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_all_tiles"
,
responseHandler
,
QJsonObject
());
// wait for response.
auto
tStart
=
hrc
::
now
();
bool
abort
=
true
;
do
{
auto
status
=
future_response
.
wait_for
(
std
::
chrono
::
milliseconds
(
100
));
if
(
status
==
std
::
future_status
::
ready
)
{
abort
=
false
;
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
);
}
// transaction error?
auto
pArray
=
future_response
.
get
();
if
(
pArray
==
nullptr
)
{
return
QVariant
(
false
);
}
// 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
->
_setTiles
(
pArray
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
());
}
QVariant
NemoInterface
::
Impl
::
_callGetVersion
()
{
this
->
_lastCall
=
CALL_NAME
::
GET_VERSION
;
// create response handler.
typedef
QString
ResponseType
;
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
const
char
*
const
versionKey
=
"version"
;
// check if transaction was successfull
if
(
o
.
contains
(
versionKey
)
&&
o
[
versionKey
].
isString
())
{
const
auto
version
=
o
[
versionKey
].
toString
();
promise_response
->
set_value
(
version
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_version no version key or wrong type."
;
promise_response
->
set_value
(
"error!"
);
}
};
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_version"
,
responseHandler
,
QJsonObject
());
// wait for response.
auto
tStart
=
hrc
::
now
();
bool
abort
=
true
;
do
{
auto
status
=
future_response
.
wait_for
(
std
::
chrono
::
milliseconds
(
100
));
if
(
status
==
std
::
future_status
::
ready
)
{
abort
=
false
;
break
;
}
}
while
(
hrc
::
now
()
-
tStart
<
maxResponseTime
||
this
->
_dispatcher
.
isInterruptionRequested
());
if
(
abort
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"all_remove_tiles(): Websocket not responding to request."
;
return
QVariant
(
false
);
}
// transaction error?
auto
version
=
future_response
.
get
();
if
(
version
==
"error!"
)
{
return
QVariant
(
false
);
}
// remote tiles (_remoteTiles)
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
version
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_setVersion
(
version
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
// return success
return
QVariant
(
future
.
get
());
}
QString
NemoInterface
::
Impl
::
_toString
(
NemoInterface
::
Impl
::
CALL_NAME
name
)
{
switch
(
name
)
{
case
CALL_NAME
:
:
ADD_TILES
:
return
QString
(
"ADD_TILES"
);
case
CALL_NAME
:
:
REMOVE_TILES
:
...
...
@@ -1130,6 +1498,10 @@ QString NemoInterface::Impl::_toString(NemoInterface::Impl::CALL_NAME name) {
return
QString