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
9e620065
Commit
9e620065
authored
Jan 25, 2021
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
new rosbridge impl added, not tested
parent
b94dfebd
Changes
6
Hide whitespace changes
Inline
Side-by-side
Showing
6 changed files
with
1212 additions
and
293 deletions
+1212
-293
qgroundcontrol.pro
qgroundcontrol.pro
+7
-12
NemoInterface.cpp
src/MeasurementComplexItem/NemoInterface.cpp
+250
-281
rosbridge.cpp
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
+253
-0
rosbridge.h
src/MeasurementComplexItem/rosbridge/rosbridge.h
+110
-0
rosbridgeimpl.cpp
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp
+508
-0
rosbridgeimpl.h
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h
+84
-0
No files found.
qgroundcontrol.pro
View file @
9e620065
...
...
@@ -267,7 +267,8 @@ QT += \
widgets
\
xml
\
texttospeech
\
core
-
private
core
-
private
\
websockets
#
Multimedia
only
used
if
QVC
is
enabled
!
contains
(
DEFINES
,
QGC_DISABLE_UVC
)
{
...
...
@@ -420,6 +421,7 @@ INCLUDEPATH += \
src
/
Audio
\
src
/
comm
\
src
/
MeasurementComplexItem
\
src
/
MeasurementComplexItem
/
rosbridge
\
src
/
MeasurementComplexItem
/
geometry
\
src
/
MeasurementComplexItem
/
nemo_interface
\
src
/
comm
/
ros_bridge
\
...
...
@@ -454,6 +456,8 @@ HEADERS += \
src
/
MeasurementComplexItem
/
geometry
/
TileDiff
.
h
\
src
/
MeasurementComplexItem
/
geometry
/
geometry
.
h
\
src
/
MeasurementComplexItem
/
HashFunctions
.
h
\
src
/
MeasurementComplexItem
/
rosbridge
/
rosbridge
.
h
\
src
/
MeasurementComplexItem
/
rosbridge
/
rosbridgeimpl
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
FutureWatcher
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
FutureWatcherInterface
.
h
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
h
\
...
...
@@ -508,18 +512,13 @@ HEADERS += \
src
/
GPS
/
Drivers
/
src
/
base_station
.
h
\
src
/
Settings
/
WimaSettings
.
h
\
src
/
comm
/
QmlObjectListHelper
.
h
\
src
/
comm
/
ros_bridge
/
include
/
RosBridgeClient
.
h
\
src
/
comm
/
ros_bridge
/
include
/
com_private
.
h
\
src
/
comm
/
ros_bridge
/
include
/
message_traits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
h
\
src
/
comm
/
ros_bridge
/
include
/
server
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
time
.
h
\
src
/
comm
/
ros_bridge
/
include
/
topic_publisher
.
h
\
src
/
comm
/
ros_bridge
/
include
/
topic_subscriber
.
h
\
src
/
comm
/
utilities
.
h
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
...
...
@@ -534,6 +533,8 @@ SOURCES += \
src
/
MeasurementComplexItem
/
geometry
/
SafeArea
.
cc
\
src
/
MeasurementComplexItem
/
geometry
/
geometry
.
cpp
\
src
/
MeasurementComplexItem
/
HashFunctions
.
cpp
\
src
/
MeasurementComplexItem
/
rosbridge
/
rosbridge
.
cpp
\
src
/
MeasurementComplexItem
/
rosbridge
/
rosbridgeimpl
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
FutureWatcherInterface
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
MeasurementTile
.
cpp
\
src
/
MeasurementComplexItem
/
nemo_interface
/
Task
.
cpp
\
...
...
@@ -554,8 +555,6 @@ SOURCES += \
src
/
MeasurementComplexItem
/
geometry
/
GeoPoint3D
.
cpp
\
src
/
MeasurementComplexItem
/
NemoInterface
.
cpp
\
src
/
comm
/
QmlObjectListHelper
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
RosBridgeClient
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
com_private
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
cpp
\
...
...
@@ -563,11 +562,7 @@ SOURCES += \
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
time
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
server
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
topic_publisher
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
topic_subscriber
.
cpp
\
src
/
Settings
/
WimaSettings
.
cc
\
src
/
comm
/
ros_bridge
/
src
/
ros_bridge
.
cpp
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
SOURCES
+=
\
...
...
src/MeasurementComplexItem/NemoInterface.cpp
View file @
9e620065
...
...
@@ -10,6 +10,7 @@
#include <shared_mutex>
#include <QTimer>
#include <QUrl>
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
...
...
@@ -19,26 +20,26 @@
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.h"
#include "ros_bridge/include/messages/nemo_msgs/progress_array.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#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/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "rosbridge/rosbridge.h"
QGC_LOGGING_CATEGORY
(
NemoInterfaceLog
,
"NemoInterfaceLog"
)
#define SYNC_INTERVAL 1000 // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
#define CONNECTION_TIMER_INTERVAL 1000 // ms
#define SYNC_INTERVAL 1000 // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
static
constexpr
auto
maxResponseTime
=
std
::
chrono
::
milliseconds
(
10000
);
static
const
char
*
progressTopic
=
"/nemo/progress"
;
static
const
char
*
heartbeatTopic
=
"/nemo/heartbeat"
;
using
hrc
=
std
::
chrono
::
high_resolution_clock
;
using
ROSBridgePtr
=
std
::
shared_ptr
<
Rosbridge
WsClient
>
;
using
ROSBridgePtr
=
std
::
shared_ptr
<
Rosbridge
>
;
typedef
ros_bridge
::
messages
::
nemo_msgs
::
tile
::
GenericTile
<
QGeoCoordinate
,
QList
>
...
...
@@ -103,6 +104,7 @@ private:
bool
_sysSync
()
const
;
// thread safe
void
_onFutureWatcherFinished
();
// thread safe
void
_onHeartbeatTimeout
();
// thread safe
void
_onRosbridgeStateChanged
();
// called from dispatcher thread!
QVariant
_callAddTiles
(
...
...
@@ -150,7 +152,7 @@ private:
std
::
atomic
<
STATE
>
_state
;
std
::
atomic
<
CALL_NAME
>
_lastCall
;
ROSBridgePtr
_pRos
B
ridge
;
ROSBridgePtr
_pRos
b
ridge
;
TileMap
_remoteTiles
;
TileMapConst
_localTiles
;
NemoInterface
*
const
_parent
;
...
...
@@ -158,7 +160,6 @@ private:
QString
_infoString
;
QString
_warningString
;
QTimer
_timeoutTimer
;
QTimer
_connectionTimer
;
QNemoHeartbeat
_lastHeartbeat
;
FutureWatcher
_futureWatcher
;
};
...
...
@@ -185,17 +186,11 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto
connectionStringFact
=
wimaSettings
->
rosbridgeConnectionString
();
auto
setConnectionString
=
[
connectionStringFact
,
this
]
{
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
if
(
!
is_valid_port_path
(
connectionString
.
toLocal8Bit
().
data
()))
{
qgcApp
()
->
warningMessageBoxOnMainThread
(
"Nemo Interface"
,
"Websocket connection string possibly invalid: "
+
connectionString
+
". Trying to connect anyways."
);
}
bool
wasRunning
=
this
->
running
();
this
->
stop
();
this
->
_pRos
Bridge
=
std
::
make_shared
<
RosbridgeWsClient
>
(
connectionString
.
toLocal8Bit
().
data
(
));
this
->
_pRos
bridge
=
std
::
make_shared
<
Rosbridge
>
(
QUrl
(
QString
(
"ws://"
)
+
connectionString
.
toLocal8Bit
().
data
()
));
if
(
wasRunning
)
{
this
->
start
();
}
...
...
@@ -209,29 +204,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
std
::
bind
(
&
Impl
::
_onHeartbeatTimeout
,
this
));
// Connection timer (temporary workaround)
connect
(
&
this
->
_connectionTimer
,
&
QTimer
::
timeout
,
[
this
]
{
if
(
this
->
_pRosBridge
->
connected
())
{
if
(
this
->
_state
==
STATE
::
START_BRIDGE
||
this
->
_state
==
STATE
::
WEBSOCKET_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_DETECTED
);
this
->
_doAction
();
}
}
else
{
if
(
this
->
_state
==
STATE
::
TRY_TOPIC_SERVICE_SETUP
||
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
WEBSOCKET_DETECTED
||
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_TIMEOUT
);
this
->
_doAction
();
}
}
});
connect
(
this
->
_pRosbridge
.
get
(),
&
Rosbridge
::
stateChanged
,
[
this
]
{
this
->
_onRosbridgeStateChanged
();
});
connect
(
&
this
->
_futureWatcher
,
&
FutureWatcher
::
finished
,
[
this
]
{
this
->
_onFutureWatcherFinished
();
});
}
NemoInterface
::
Impl
::~
Impl
()
{
this
->
_pRos
Bridge
->
reset
();
}
NemoInterface
::
Impl
::~
Impl
()
{
this
->
_pRos
bridge
->
stop
();
}
void
NemoInterface
::
Impl
::
start
()
{
if
(
!
running
())
{
...
...
@@ -533,6 +513,25 @@ void NemoInterface::Impl::_onHeartbeatTimeout() {
this
->
_doAction
();
}
void
NemoInterface
::
Impl
::
_onRosbridgeStateChanged
()
{
auto
state
=
this
->
_pRosbridge
->
state
();
if
(
state
==
Rosbridge
::
STATE
::
CONNECTED
)
{
if
(
this
->
_state
==
STATE
::
START_BRIDGE
||
this
->
_state
==
STATE
::
WEBSOCKET_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_DETECTED
);
this
->
_doAction
();
}
}
else
if
(
state
==
Rosbridge
::
STATE
::
TIMEOUT
)
{
if
(
this
->
_state
==
STATE
::
TRY_TOPIC_SERVICE_SETUP
||
this
->
_state
==
STATE
::
READY
||
this
->
_state
==
STATE
::
WEBSOCKET_DETECTED
||
this
->
_state
==
STATE
::
HEARTBEAT_TIMEOUT
)
{
this
->
_setState
(
STATE
::
WEBSOCKET_TIMEOUT
);
this
->
_doAction
();
}
}
}
bool
NemoInterface
::
Impl
::
_userSync
()
const
{
return
_userSync
(
this
->
_state
);
}
const
QString
&
NemoInterface
::
Impl
::
infoString
()
const
{
return
_infoString
;
}
...
...
@@ -600,113 +599,101 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
using
namespace
ros_bridge
::
messages
;
// Subscribe nemo progress.
const
char
*
progressClient
=
"client:/nemo/progress"
;
this
->
_pRosBridge
->
addClient
(
progressClient
);
this
->
_pRosBridge
->
subscribe
(
progressClient
,
"/nemo/progress"
,
[
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
auto
msg
=
in_message
->
string
();
// parse in_message
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
c_str
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"msg"
)
&&
d
[
"msg"
].
IsObject
())
{
// create obj from json
nemo_msgs
::
progress_array
::
ProgressArray
progressArray
;
if
(
nemo_msgs
::
progress_array
::
fromJson
(
d
[
"msg"
],
progressArray
))
{
// correct range errors of progress
for
(
auto
&
lp
:
progressArray
.
progress_array
())
{
bool
rangeError
=
false
;
if
(
lp
.
progress
()
<
0
)
{
lp
.
setProgress
(
0
);
rangeError
=
true
;
}
if
(
lp
.
progress
()
>
100
)
{
lp
.
setProgress
(
100
);
rangeError
=
true
;
}
if
(
rangeError
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress progress out "
"of range, value was set to: "
<<
lp
.
progress
();
}
}
auto
p
=
std
::
make_shared
<
ProgressArray
>
();
*
p
=
std
::
move
(
progressArray
.
progress_array
());
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
p
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_updateProgress
(
p
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
future
.
wait
();
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress not able to "
"create ProgressArray form json: "
<<
msg
.
c_str
();
this
->
_pRosbridge
->
subscribeTopic
(
progressTopic
,
[
this
](
const
QJsonObject
&
o
)
{
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
// parse in_message
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
toUtf8
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"msg"
)
&&
d
[
"msg"
].
IsObject
())
{
// create obj from json
nemo_msgs
::
progress_array
::
ProgressArray
progressArray
;
if
(
nemo_msgs
::
progress_array
::
fromJson
(
d
[
"msg"
],
progressArray
))
{
// correct range errors of progress
for
(
auto
&
lp
:
progressArray
.
progress_array
())
{
bool
rangeError
=
false
;
if
(
lp
.
progress
()
<
0
)
{
lp
.
setProgress
(
0
);
rangeError
=
true
;
}
if
(
lp
.
progress
()
>
100
)
{
lp
.
setProgress
(
100
);
rangeError
=
true
;
}
if
(
rangeError
)
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress progress out "
"of range, value was set to: "
<<
lp
.
progress
();
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress no
\"
msg
\"
key or wrong type: "
<<
msg
.
c_str
();
}
auto
p
=
std
::
make_shared
<
ProgressArray
>
();
*
p
=
std
::
move
(
progressArray
.
progress_array
());
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
p
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_updateProgress
(
p
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
future
.
wait
();
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
.
c_str
()
;
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress not able to "
"create ProgressArray form json: "
<<
msg
;
}
});
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress no
\"
msg
\"
key or wrong type: "
<<
msg
;
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
;
}
});
// Subscribe heartbeat msg.
const
char
*
heartbeatClient
=
"client:/nemo/heartbeat"
;
this
->
_pRosBridge
->
addClient
(
heartbeatClient
);
this
->
_pRosBridge
->
subscribe
(
heartbeatClient
,
"/nemo/heartbeat"
,
[
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
auto
msg
=
in_message
->
string
();
// parse in_message
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
c_str
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"msg"
)
&&
d
[
"msg"
].
IsObject
())
{
// create obj from json
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
d
[
"msg"
],
heartbeat
))
{
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
heartbeat
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_onHeartbeatReceived
(
heartbeat
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
future
.
wait
();
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat not able to "
"create Heartbeat form json: "
<<
msg
.
c_str
();
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat no
\"
msg
\"
key or wrong type: "
<<
msg
.
c_str
();
}
this
->
_pRosbridge
->
subscribeTopic
(
heartbeatTopic
,
[
this
](
const
QJsonObject
&
o
)
{
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
// parse in_message
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
toUtf8
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"msg"
)
&&
d
[
"msg"
].
IsObject
())
{
// create obj from json
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
d
[
"msg"
],
heartbeat
))
{
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
this
->
_parent
,
[
this
,
heartbeat
,
promise
=
std
::
move
(
promise
)]()
mutable
{
this
->
_onHeartbeatReceived
(
heartbeat
,
std
::
move
(
promise
));
});
Q_ASSERT
(
value
==
true
);
future
.
wait
();
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
.
c_str
()
;
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat not able to "
"create Heartbeat form json: "
<<
msg
;
}
});
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat no
\"
msg
\"
key or wrong type: "
<<
msg
;
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
;
}
});
}
void
NemoInterface
::
Impl
::
_trySynchronize
()
{
...
...
@@ -778,17 +765,15 @@ void NemoInterface::Impl::_doAction() {
static
bool
resetDone
=
false
;
switch
(
this
->
_state
)
{
case
STATE
:
:
STOPPED
:
this
->
_connectionTimer
.
stop
();
this
->
_timeoutTimer
.
stop
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
if
(
this
->
_pRos
Bridge
->
running
()
)
{
this
->
_pRos
Bridge
->
reset
();
if
(
this
->
_pRos
bridge
->
state
()
!=
Rosbridge
::
STATE
::
STOPPED
)
{
this
->
_pRos
bridge
->
stop
();
}
break
;
case
STATE
:
:
START_BRIDGE
:
this
->
_pRosBridge
->
run
();
this
->
_connectionTimer
.
start
(
CONNECTION_TIMER_INTERVAL
);
this
->
_pRosbridge
->
start
();
break
;
break
;
case
STATE
:
:
WEBSOCKET_DETECTED
:
...
...
@@ -812,8 +797,8 @@ void NemoInterface::Impl::_doAction() {
case
STATE
:
:
WEBSOCKET_TIMEOUT
:
if
(
!
resetDone
)
{
resetDone
=
true
;
this
->
_pRos
Bridge
->
reset
();
this
->
_pRos
Bridge
->
run
();
this
->
_pRos
bridge
->
stop
();
this
->
_pRos
bridge
->
start
();
}
this
->
_timeoutTimer
.
stop
();
this
->
_clearTilesRemote
(
std
::
promise
<
bool
>
());
...
...
@@ -843,48 +828,48 @@ QVariant NemoInterface::Impl::_callAddTiles(
}
jsonTileArray
.
PushBack
(
jsonTile
,
allocator
);
}
// for
rapidjson
::
Value
tileKey
(
"in_tile_array"
);
request
.
AddMember
(
tileKey
,
jsonTileArray
,
allocator
);
rapidjson
::
StringBuffer
buffer
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
buffer
);
request
.
Accept
(
writer
);
QJsonDocument
req
=
QJsonDocument
::
fromJson
(
buffer
.
GetString
());
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
mutable
{
// check if transaction was successfull
auto
msg
=
in_message
->
string
();
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
c_str
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"values"
)
&&
d
[
"values"
].
IsObject
())
{
auto
values
=
d
[
"values"
].
GetObject
();
if
(
values
.
HasMember
(
"success"
)
&&
values
[
"success"
].
IsBool
())
{
promise_response
->
set_value
(
values
[
"success"
].
GetBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles no
\"
success
\"
key or wrong type: "
<<
msg
.
c_str
();
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles no
\"
values
\"
key or wrong type: "
<<
msg
.
c_str
();
promise_response
->
set_value
(
false
);
}
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
toUtf8
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"values"
)
&&
d
[
"values"
].
IsObject
())
{
auto
values
=
d
[
"values"
].
GetObject
();
if
(
values
.
HasMember
(
"success"
)
&&
values
[
"success"
].
IsBool
())
{
promise_response
->
set_value
(
values
[
"success"
].
GetBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
.
c_str
();
<<
"/nemo/add_tiles no
\"
success
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
connection
->
send_close
(
1000
);
};
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles no
\"
values
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
;
promise_response
->
set_value
(
false
);
}
};
// call service.
this
->
_pRosBridge
->
callService
(
"/nemo/add_tiles"
,
responseHandler
,
request
);
this
->
_pRosbridge
->
callService
(
"/nemo/add_tiles"
,
responseHandler
,
req
.
object
());
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -940,46 +925,44 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
rapidjson
::
Value
tileKey
(
"ids"
);
request
.
AddMember
(
tileKey
,
jsonIdArray
,
allocator
);
rapidjson
::
StringBuffer
buffer
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
buffer
);
request
.
Accept
(
writer
);
QJsonDocument
req
=
QJsonDocument
::
fromJson
(
buffer
.
GetString
());
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
mutable
{
// check if transaction was successfull
auto
msg
=
in_message
->
string
();
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
c_str
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"values"
)
&&
d
[
"values"
].
IsObject
())
{
auto
values
=
d
[
"values"
].
GetObject
();
if
(
values
.
HasMember
(
"success"
)
&&
values
[
"success"
].
IsBool
())
{
promise_response
->
set_value
(
values
[
"success"
].
GetBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles no
\"
success
\"
key or wrong type: "
<<
msg
.
c_str
();
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles no
\"
values
\"
key or wrong type: "
<<
msg
.
c_str
();
promise_response
->
set_value
(
false
);
}
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
toUtf8
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"values"
)
&&
d
[
"values"
].
IsObject
())
{
auto
values
=
d
[
"values"
].
GetObject
();
if
(
values
.
HasMember
(
"success"
)
&&
values
[
"success"
].
IsBool
())
{
promise_response
->
set_value
(
values
[
"success"
].
GetBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
.
c_str
();
<<
"/nemo/remove_tiles no
\"
success
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
connection
->
send_close
(
1000
);
};
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles no
\"
values
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
;
promise_response
->
set_value
(
false
);
}
};
// call service.
this
->
_pRos
B
ridge
->
callService
(
"/nemo/remove_tiles"
,
responseHandler
,
req
uest
);
this
->
_pRos
b
ridge
->
callService
(
"/nemo/remove_tiles"
,
responseHandler
,
req
.
object
()
);
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1026,35 +1009,29 @@ 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
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
mutable
{
// check if transaction was successfull
auto
msg
=
in_message
->
string
();
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
c_str
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"result"
)
&&
d
[
"result"
].
IsBool
())
{
promise_response
->
set_value
(
d
[
"result"
].
GetBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/clear_tiles no
\"
result
\"
key or wrong type: "
<<
msg
.
c_str
();
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/clear_tiles message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
.
c_str
();
promise_response
->
set_value
(
false
);
}
connection
->
send_close
(
1000
);
};
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
toUtf8
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"result"
)
&&
d
[
"result"
].
IsBool
())
{
promise_response
->
set_value
(
d
[
"result"
].
GetBool
());
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/clear_tiles no
\"
result
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/clear_tiles message parse error ("
<<
d
.
GetParseError
()
<<
"): "
<<
msg
;
promise_response
->
set_value
(
false
);
}
};
// call service.
this
->
_pRos
B
ridge
->
callService
(
"/nemo/clear_tiles"
,
responseHandler
,
rapidjson
::
Document
(
rapidjson
::
kObjectType
));
this
->
_pRos
b
ridge
->
callService
(
"/nemo/clear_tiles"
,
responseHandler
,
QJsonObject
(
));
// wait for response.
auto
tStart
=
hrc
::
now
();
...
...
@@ -1108,52 +1085,51 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
rapidjson
::
Value
tileKey
(
"ids"
);
request
.
AddMember
(
tileKey
,
jsonIdArray
,
allocator
);
rapidjson
::
StringBuffer
buffer
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
buffer
);
request
.
Accept
(
writer
);
QJsonDocument
req
=
QJsonDocument
::
fromJson
(
buffer
.
GetString
());
// create response handler.
typedef
std
::
shared_ptr
<
ProgressArray
>
ResponseType
;
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
ResponseType
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
mutable
{
// check if transaction was successfull
auto
msg
=
in_message
->
string
();
rapidjson
::
Document
d
;
d
.
Parse
(
msg
.
c_str
());
if
(
!
d
.
HasParseError
())
{
if
(
d
.
HasMember
(
"values"
)
&&
d
[
"values"
].
IsObject
())
{
auto
values
=
d
[
"values"
].
GetObject
();
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
progressArrayMsg
;
if
(
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
fromJson
(
d
[
"values"
],
progressArrayMsg
))
{
auto
pArray
=
std
::
make_shared
<
ProgressArray
>
();
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
promise_response
->
set_value
(
pArray
);
}
else
{