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
b098b99a
Commit
b098b99a
authored
Jan 26, 2021
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
tile id int64_t > QString, for robustness
parent
9e620065
Changes
30
Hide whitespace changes
Inline
Side-by-side
Showing
30 changed files
with
326 additions
and
1084 deletions
+326
-1084
qgroundcontrol.pro
qgroundcontrol.pro
+0
-4
IDArray.h
src/MeasurementComplexItem/IDArray.h
+1
-1
MeasurementComplexItem.h
src/MeasurementComplexItem/MeasurementComplexItem.h
+1
-1
NemoInterface.cpp
src/MeasurementComplexItem/NemoInterface.cpp
+65
-200
MeasurementArea.cc
src/MeasurementComplexItem/geometry/MeasurementArea.cc
+17
-8
MeasurementArea.h
src/MeasurementComplexItem/geometry/MeasurementArea.h
+1
-1
MeasurementTile.cpp
...MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
+16
-3
MeasurementTile.h
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
+5
-3
TaskDispatcher.cpp
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp
+2
-0
TaskDispatcher.h
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h
+1
-0
rosbridge.cpp
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
+33
-14
rosbridge.h
src/MeasurementComplexItem/rosbridge/rosbridge.h
+3
-0
rosbridgeimpl.cpp
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp
+7
-3
geopoint.h
...mm/ros_bridge/include/messages/geographic_msgs/geopoint.h
+108
-115
point32.cpp
...omm/ros_bridge/include/messages/geometry_msgs/point32.cpp
+0
-6
point32.h
src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
+0
-139
polygon.cpp
...omm/ros_bridge/include/messages/geometry_msgs/polygon.cpp
+0
-6
polygon.h
src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
+0
-91
polygon_stamped.cpp
...bridge/include/messages/geometry_msgs/polygon_stamped.cpp
+0
-6
polygon_stamped.h
...s_bridge/include/messages/geometry_msgs/polygon_stamped.h
+0
-172
heartbeat.h
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+6
-7
labeled_progress.cpp
...os_bridge/include/messages/nemo_msgs/labeled_progress.cpp
+1
-1
labeled_progress.h
.../ros_bridge/include/messages/nemo_msgs/labeled_progress.h
+17
-17
progress_array.h
...mm/ros_bridge/include/messages/nemo_msgs/progress_array.h
+16
-16
tile.cpp
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp
+1
-1
tile.h
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
+25
-28
header.cpp
src/comm/ros_bridge/include/messages/std_msgs/header.cpp
+0
-69
header.h
src/comm/ros_bridge/include/messages/std_msgs/header.h
+0
-102
time.cpp
src/comm/ros_bridge/include/messages/std_msgs/time.cpp
+0
-5
time.h
src/comm/ros_bridge/include/messages/std_msgs/time.h
+0
-65
No files found.
qgroundcontrol.pro
View file @
b098b99a
...
@@ -517,8 +517,6 @@ HEADERS += \
...
@@ -517,8 +517,6 @@ HEADERS += \
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
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
/
progress_array
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
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
/
messages
/
std_msgs
/
time
.
h
\
src
/
comm
/
utilities
.
h
src
/
comm
/
utilities
.
h
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
...
@@ -560,8 +558,6 @@ SOURCES += \
...
@@ -560,8 +558,6 @@ SOURCES += \
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
labeled_progress
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
time
.
cpp
\
src
/
Settings
/
WimaSettings
.
cc
\
src
/
Settings
/
WimaSettings
.
cc
\
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
...
...
src/MeasurementComplexItem/IDArray.h
View file @
b098b99a
...
@@ -3,6 +3,6 @@
...
@@ -3,6 +3,6 @@
#include <QVector>
#include <QVector>
typedef
QVector
<
std
::
int64_t
>
IDArray
;
typedef
QVector
<
QString
>
IDArray
;
#endif // IDARRAY_H
#endif // IDARRAY_H
src/MeasurementComplexItem/MeasurementComplexItem.h
View file @
b098b99a
...
@@ -10,7 +10,7 @@
...
@@ -10,7 +10,7 @@
#include "SettingsFact.h"
#include "SettingsFact.h"
#include "AreaData.h"
#include "AreaData.h"
#include "ProgressArray.h"
#include "
geometry/
ProgressArray.h"
class
RoutingThread
;
class
RoutingThread
;
class
RoutingResult
;
class
RoutingResult
;
...
...
src/MeasurementComplexItem/NemoInterface.cpp
View file @
b098b99a
...
@@ -9,6 +9,8 @@
...
@@ -9,6 +9,8 @@
#include <shared_mutex>
#include <shared_mutex>
#include <QJsonArray>
#include <QJsonObject>
#include <QTimer>
#include <QTimer>
#include <QUrl>
#include <QUrl>
...
@@ -24,9 +26,6 @@
...
@@ -24,9 +26,6 @@
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.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/progress_array.h"
#include "ros_bridge/include/messages/nemo_msgs/tile.h"
#include "ros_bridge/include/messages/nemo_msgs/tile.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"
#include "rosbridge/rosbridge.h"
QGC_LOGGING_CATEGORY
(
NemoInterfaceLog
,
"NemoInterfaceLog"
)
QGC_LOGGING_CATEGORY
(
NemoInterfaceLog
,
"NemoInterfaceLog"
)
...
@@ -44,8 +43,8 @@ using ROSBridgePtr = std::shared_ptr<Rosbridge>;
...
@@ -44,8 +43,8 @@ using ROSBridgePtr = std::shared_ptr<Rosbridge>;
typedef
ros_bridge
::
messages
::
nemo_msgs
::
tile
::
GenericTile
<
QGeoCoordinate
,
typedef
ros_bridge
::
messages
::
nemo_msgs
::
tile
::
GenericTile
<
QGeoCoordinate
,
QList
>
QList
>
Tile
;
Tile
;
typedef
std
::
map
<
std
::
int64_t
,
std
::
shared_ptr
<
Tile
>>
TileMap
;
typedef
std
::
map
<
QString
,
std
::
shared_ptr
<
Tile
>>
TileMap
;
typedef
std
::
map
<
std
::
int64_t
,
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
nemo_interface
::
FutureWatcher
<
QVariant
,
std
::
shared_future
>
...
@@ -599,19 +598,10 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
...
@@ -599,19 +598,10 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
using
namespace
ros_bridge
::
messages
;
using
namespace
ros_bridge
::
messages
;
// Subscribe nemo progress.
// Subscribe nemo progress.
this
->
_pRosbridge
->
subscribeTopic
(
progressTopic
,
[
this
](
this
->
_pRosbridge
->
subscribeTopic
(
const
QJsonObject
&
o
)
{
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
;
nemo_msgs
::
progress_array
::
ProgressArray
progressArray
;
if
(
nemo_msgs
::
progress_array
::
fromJson
(
d
[
"msg"
]
,
progressArray
))
{
if
(
nemo_msgs
::
progress_array
::
fromJson
(
o
,
progressArray
))
{
// correct range errors of progress
// correct range errors of progress
for
(
auto
&
lp
:
progressArray
.
progress_array
())
{
for
(
auto
&
lp
:
progressArray
.
progress_array
())
{
...
@@ -645,32 +635,15 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
...
@@ -645,32 +635,15 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress not able to "
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/progress not able to "
"create ProgressArray form json: "
"create ProgressArray form json: "
<<
msg
;
<<
o
;
}
}
}
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.
// Subscribe heartbeat msg.
this
->
_pRosbridge
->
subscribeTopic
(
heartbeatTopic
,
[
this
](
this
->
_pRosbridge
->
subscribeTopic
(
const
QJsonObject
&
o
)
{
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
;
nemo_msgs
::
heartbeat
::
Heartbeat
heartbeat
;
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
d
[
"msg"
]
,
heartbeat
))
{
if
(
nemo_msgs
::
heartbeat
::
fromJson
(
o
,
heartbeat
))
{
std
::
promise
<
bool
>
promise
;
std
::
promise
<
bool
>
promise
;
auto
future
=
promise
.
get_future
();
auto
future
=
promise
.
get_future
();
bool
value
=
QMetaObject
::
invokeMethod
(
bool
value
=
QMetaObject
::
invokeMethod
(
...
@@ -683,17 +656,9 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
...
@@ -683,17 +656,9 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat not able to "
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/heartbeat not able to "
"create Heartbeat form json: "
"create Heartbeat form json: "
<<
msg
;
<<
o
;
}
}
}
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
()
{
void
NemoInterface
::
Impl
::
_trySynchronize
()
{
...
@@ -701,6 +666,11 @@ void NemoInterface::Impl::_trySynchronize() {
...
@@ -701,6 +666,11 @@ void NemoInterface::Impl::_trySynchronize() {
this
->
_state
==
STATE
::
USER_SYNC
)
&&
this
->
_state
==
STATE
::
USER_SYNC
)
&&
!
_isSynchronized
())
{
!
_isSynchronized
())
{
if
(
!
_dispatcher
.
idle
())
{
QTimer
::
singleShot
(
5000
,
[
this
]
{
this
->
_trySynchronize
();
});
return
;
}
qCWarning
(
NemoInterfaceLog
)
<<
"trying to synchronize"
;
qCWarning
(
NemoInterfaceLog
)
<<
"trying to synchronize"
;
this
->
_setState
(
STATE
::
SYS_SYNC
);
this
->
_setState
(
STATE
::
SYS_SYNC
);
...
@@ -711,8 +681,6 @@ void NemoInterface::Impl::_trySynchronize() {
...
@@ -711,8 +681,6 @@ void NemoInterface::Impl::_trySynchronize() {
std
::
bind
(
&
Impl
::
_callClearTiles
,
this
));
std
::
bind
(
&
Impl
::
_callClearTiles
,
this
));
// dispatch command.
// dispatch command.
_dispatcher
.
clear
();
_dispatcher
.
stop
();
Q_ASSERT
(
_dispatcher
.
pendingTasks
()
==
0
);
Q_ASSERT
(
_dispatcher
.
pendingTasks
()
==
0
);
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
ret
=
_dispatcher
.
dispatch
(
std
::
move
(
pTask
));
auto
clearFuture
=
ret
.
share
();
auto
clearFuture
=
ret
.
share
();
...
@@ -775,7 +743,6 @@ void NemoInterface::Impl::_doAction() {
...
@@ -775,7 +743,6 @@ void NemoInterface::Impl::_doAction() {
case
STATE
:
:
START_BRIDGE
:
case
STATE
:
:
START_BRIDGE
:
this
->
_pRosbridge
->
start
();
this
->
_pRosbridge
->
start
();
break
;
break
;
break
;
case
STATE
:
:
WEBSOCKET_DETECTED
:
case
STATE
:
:
WEBSOCKET_DETECTED
:
resetDone
=
false
;
resetDone
=
false
;
this
->
_setState
(
STATE
::
TRY_TOPIC_SERVICE_SETUP
);
this
->
_setState
(
STATE
::
TRY_TOPIC_SERVICE_SETUP
);
...
@@ -814,62 +781,38 @@ QVariant NemoInterface::Impl::_callAddTiles(
...
@@ -814,62 +781,38 @@ QVariant NemoInterface::Impl::_callAddTiles(
this
->
_lastCall
=
CALL_NAME
::
ADD_TILES
;
this
->
_lastCall
=
CALL_NAME
::
ADD_TILES
;
// create json object
// create json object
rapidjson
::
Document
request
(
rapidjson
::
kObjectType
);
QJsonArray
jsonTileArray
;
auto
&
allocator
=
request
.
GetAllocator
();
rapidjson
::
Value
jsonTileArray
(
rapidjson
::
kArrayType
);
for
(
const
auto
&
tile
:
*
pTileArray
)
{
for
(
const
auto
&
tile
:
*
pTileArray
)
{
using
namespace
ros_bridge
::
messages
;
using
namespace
ros_bridge
::
messages
;
rapidjson
::
Value
jsonTile
(
rapidjson
::
kObjectType
)
;
QJsonObject
jsonTile
;
if
(
!
nemo_msgs
::
tile
::
toJson
(
*
tile
,
jsonTile
,
allocator
))
{
if
(
!
nemo_msgs
::
tile
::
toJson
(
*
tile
,
jsonTile
))
{
qCDebug
(
NemoInterfaceLog
)
qCDebug
(
NemoInterfaceLog
)
<<
"addTiles(): not able to create json object: tile id: "
<<
"addTiles(): not able to create json object: tile id: "
<<
tile
->
id
()
<<
" progress: "
<<
tile
->
progress
()
<<
tile
->
id
()
<<
" progress: "
<<
tile
->
progress
()
<<
" points: "
<<
tile
->
tile
();
<<
" points: "
<<
tile
->
tile
();
}
}
jsonTileArray
.
PushBack
(
jsonTile
,
allocator
);
jsonTileArray
.
append
(
std
::
move
(
jsonTile
)
);
}
// for
}
// for
rapidjson
::
Value
tileKey
(
"in_tile_array"
);
QJsonObject
req
;
request
.
AddMember
(
tileKey
,
jsonTileArray
,
allocator
);
req
[
"in_tile_array"
]
=
std
::
move
(
jsonTileArray
);
rapidjson
::
StringBuffer
buffer
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
buffer
);
request
.
Accept
(
writer
);
QJsonDocument
req
=
QJsonDocument
::
fromJson
(
buffer
.
GetString
());
// create response handler.
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
auto
future_response
=
promise_response
->
get_future
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
if
(
o
.
contains
(
"success"
)
&&
o
[
"success"
].
isBool
())
{
rapidjson
::
Document
d
;
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
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 no
\"
success
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles no
\"
values
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/add_tiles message parse error ("
qCWarning
(
NemoInterfaceLog
)
<<
d
.
GetParseError
()
<<
"): "
<<
msg
;
<<
"/nemo/add_tiles 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
,
this
->
_pRosbridge
->
callService
(
"/nemo/add_tiles"
,
responseHandler
,
req
);
req
.
object
());
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -915,20 +858,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
...
@@ -915,20 +858,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
this
->
_lastCall
=
CALL_NAME
::
REMOVE_TILES
;
this
->
_lastCall
=
CALL_NAME
::
REMOVE_TILES
;
// create json object
// create json object
rapidjson
::
Document
request
(
rapidjson
::
kObjectType
);
QJsonArray
jsonIdArray
;
auto
&
allocator
=
request
.
GetAllocator
();
rapidjson
::
Value
jsonIdArray
(
rapidjson
::
kArrayType
);
for
(
const
auto
id
:
*
pIdArray
)
{
for
(
const
auto
id
:
*
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
PushBack
(
rapidjson
::
Value
(
id
),
allocator
);
jsonIdArray
.
append
(
id
);
}
// for
}
// for
rapidjson
::
Value
tileKey
(
"ids"
);
QJsonObject
req
;
request
.
AddMember
(
tileKey
,
jsonIdArray
,
allocator
);
req
[
"ids"
]
=
std
::
move
(
jsonIdArray
);
rapidjson
::
StringBuffer
buffer
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
buffer
);
request
.
Accept
(
writer
);
QJsonDocument
req
=
QJsonDocument
::
fromJson
(
buffer
.
GetString
());
// create response handler.
// create response handler.
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
auto
promise_response
=
std
::
make_shared
<
std
::
promise
<
bool
>>
();
...
@@ -936,33 +872,17 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
...
@@ -936,33 +872,17 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
rapidjson
::
Document
d
;
if
(
o
.
contains
(
"success"
)
&&
o
[
"success"
].
isBool
())
{
d
.
Parse
(
msg
.
toUtf8
());
promise_response
->
set_value
(
o
[
"success"
].
toBool
());
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
;
promise_response
->
set_value
(
false
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles no
\"
values
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
false
);
}
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/remove_tiles message parse error ("
qCWarning
(
NemoInterfaceLog
)
<<
d
.
GetParseError
()
<<
")
: "
<<
msg
;
<<
"/nemo/remove_tiles 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
,
this
->
_pRosbridge
->
callService
(
"/nemo/remove_tiles"
,
responseHandler
,
req
);
req
.
object
());
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1011,22 +931,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
...
@@ -1011,22 +931,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
auto
future_response
=
promise_response
->
get_future
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
promise_response
->
set_value
(
true
);
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.
// call service.
...
@@ -1075,20 +980,13 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
...
@@ -1075,20 +980,13 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
this
->
_lastCall
=
CALL_NAME
::
GET_PROGRESS
;
this
->
_lastCall
=
CALL_NAME
::
GET_PROGRESS
;
// create json object
// create json object
rapidjson
::
Document
request
(
rapidjson
::
kObjectType
);
QJsonArray
jsonIdArray
;
auto
&
allocator
=
request
.
GetAllocator
();
rapidjson
::
Value
jsonIdArray
(
rapidjson
::
kArrayType
);
for
(
const
auto
id
:
*
pIdArray
)
{
for
(
const
auto
id
:
*
pIdArray
)
{
using
namespace
ros_bridge
::
messages
;
using
namespace
ros_bridge
::
messages
;
jsonIdArray
.
PushBack
(
rapidjson
::
Value
(
id
),
allocator
);
jsonIdArray
.
append
(
id
);
}
// for
}
// for
rapidjson
::
Value
tileKey
(
"ids"
);
QJsonObject
req
;
request
.
AddMember
(
tileKey
,
jsonIdArray
,
allocator
);
req
[
"ids"
]
=
std
::
move
(
jsonIdArray
);
rapidjson
::
StringBuffer
buffer
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
buffer
);
request
.
Accept
(
writer
);
QJsonDocument
req
=
QJsonDocument
::
fromJson
(
buffer
.
GetString
());
// create response handler.
// create response handler.
typedef
std
::
shared_ptr
<
ProgressArray
>
ResponseType
;
typedef
std
::
shared_ptr
<
ProgressArray
>
ResponseType
;
...
@@ -1096,40 +994,23 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
...
@@ -1096,40 +994,23 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
auto
future_response
=
promise_response
->
get_future
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
rapidjson
::
Document
d
;
progressArrayMsg
;
d
.
Parse
(
msg
.
toUtf8
());
if
(
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
fromJson
(
if
(
!
d
.
HasParseError
())
{
o
,
progressArrayMsg
))
{
if
(
d
.
HasMember
(
"values"
)
&&
d
[
"values"
].
IsObject
())
{
auto
pArray
=
std
::
make_shared
<
ProgressArray
>
();
auto
values
=
d
[
"values"
].
GetObject
();
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
promise_response
->
set_value
(
pArray
);
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
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_progress error while creating ProgressArray "
"from json."
;
promise_response
->
set_value
(
nullptr
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_progress no
\"
values
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
nullptr
);
}
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/get_progress message parse error ("
qCWarning
(
NemoInterfaceLog
)
<<
d
.
GetParseError
()
<<
"): "
<<
msg
;
<<
"/nemo/get_progress error while creating ProgressArray "
"from json."
;
promise_response
->
set_value
(
nullptr
);
promise_response
->
set_value
(
nullptr
);
}
}
};
};
// call service.
// call service.
this
->
_pRosbridge
->
callService
(
"/nemo/get_progress"
,
responseHandler
,
this
->
_pRosbridge
->
callService
(
"/nemo/get_progress"
,
responseHandler
,
req
);
req
.
object
());
// wait for response.
// wait for response.
auto
tStart
=
hrc
::
now
();
auto
tStart
=
hrc
::
now
();
...
@@ -1179,34 +1060,18 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
...
@@ -1179,34 +1060,18 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
auto
future_response
=
promise_response
->
get_future
();
auto
future_response
=
promise_response
->
get_future
();
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
auto
responseHandler
=
[
promise_response
](
const
QJsonObject
&
o
)
mutable
{
// check if transaction was successfull
// check if transaction was successfull
QString
msg
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
rapidjson
::
Document
d
;
progressArrayMsg
;
d
.
Parse
(
msg
.
toUtf8
());
if
(
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
fromJson
(
if
(
!
d
.
HasParseError
())
{
o
,
progressArrayMsg
))
{
if
(
d
.
HasMember
(
"values"
)
&&
d
[
"values"
].
IsObject
())
{
auto
pArray
=
std
::
make_shared
<
ProgressArray
>
();
ros_bridge
::
messages
::
nemo_msgs
::
progress_array
::
ProgressArray
*
pArray
=
std
::
move
(
progressArrayMsg
.
progress_array
());
progressArrayMsg
;
promise_response
->
set_value
(
pArray
);
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
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/all_get_progress error while creating ProgressArray "
"from json."
;
promise_response
->
set_value
(
nullptr
);
}
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/all_get_progress no
\"
values
\"
key or wrong type: "
<<
msg
;
promise_response
->
set_value
(
nullptr
);
}
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"/nemo/all_get_progress message parse error ("
<<
d
.
GetParseError
()
<<
"/nemo/get_all_progress error while creating ProgressArray "
<<
"): "
<<
msg
;
"from json. msg: "
<<
o
;
promise_response
->
set_value
(
nullptr
);
promise_response
->
set_value
(
nullptr
);
}
}
};
};
...
...
src/MeasurementComplexItem/geometry/MeasurementArea.cc
View file @
b098b99a
...
@@ -19,6 +19,8 @@
...
@@ -19,6 +19,8 @@
#define MAX_TILES 1000
#define MAX_TILES 1000
#endif
#endif
QString
randomId
();
using
namespace
geometry
;
using
namespace
geometry
;
namespace
trans
=
bg
::
strategy
::
transform
;
namespace
trans
=
bg
::
strategy
::
transform
;
...
@@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
...
@@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
// find unique id
// find unique id
if
(
it
!=
_indexMap
.
end
())
{
if
(
it
!=
_indexMap
.
end
())
{
auto
newId
=
tile
->
id
()
+
1
;
auto
newId
=
MeasurementTile
::
randomId
()
;
constexpr
long
counterMax
=
1e6
;
constexpr
long
counterMax
=
1e6
;
unsigned
long
counter
=
0
;
unsigned
long
counter
=
0
;
for
(;
counter
<=
counterMax
;
++
counter
)
{
for
(;
counter
<=
counterMax
;
++
counter
)
{
...
@@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
...
@@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
if
(
it
==
_indexMap
.
end
())
{
if
(
it
==
_indexMap
.
end
())
{
break
;
break
;
}
else
{
}
else
{
++
newId
;
newId
=
MeasurementTile
::
randomId
()
;
}
}
}
}
...
@@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() {
...
@@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() {
// Convert to geo system.
// Convert to geo system.
for
(
const
auto
&
t
:
tilesENU
)
{
for
(
const
auto
&
t
:
tilesENU
)
{
auto
geoTile
=
new
MeasurementTile
(
pData
.
get
());
auto
geoTile
=
new
MeasurementTile
(
pData
.
get
());
std
::
size_t
hashValue
=
0
;
std
::
hash
<
QGeoCoordinate
>
hashFun
;
for
(
const
auto
&
v
:
t
.
outer
())
{
for
(
const
auto
&
v
:
t
.
outer
())
{
QGeoCoordinate
geoVertex
;
QGeoCoordinate
geoVertex
;
fromENU
(
origin
,
v
,
geoVertex
);
fromENU
(
origin
,
v
,
geoVertex
);
geoTile
->
push_back
(
geoVertex
);
geoTile
->
push_back
(
geoVertex
);
hashValue
^=
hashFun
(
geoVertex
);
}
}
geoTile
->
setId
(
std
::
int64_t
(
hashValue
));
pData
->
append
(
geoTile
);
pData
->
append
(
geoTile
);
}
}
}
}
...
@@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() {
...
@@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() {
// find unique id
// find unique id
if
(
it
!=
_indexMap
.
end
())
{
if
(
it
!=
_indexMap
.
end
())
{
auto
newId
=
tile
->
id
()
+
1
;
auto
newId
=
MeasurementTile
::
randomId
()
;
constexpr
long
counterMax
=
1e6
;
constexpr
long
counterMax
=
1e6
;
unsigned
long
counter
=
0
;
unsigned
long
counter
=
0
;
for
(;
counter
<=
counterMax
;
++
counter
)
{
for
(;
counter
<=
counterMax
;
++
counter
)
{
...
@@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() {
...
@@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() {
if
(
it
==
_indexMap
.
end
())
{
if
(
it
==
_indexMap
.
end
())
{
break
;
break
;
}
else
{
}
else
{
++
newId
;
newId
=
MeasurementTile
::
randomId
()
;
}
}
}
}
...
@@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
...
@@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
return
true
;
return
true
;
}
}
QString
randomId
()
{
std
::
srand
(
std
::
time
(
nullptr
));
std
::
int64_t
r
=
0
;
for
(
int
i
=
0
;
i
<
10
;
++
i
)
{
r
^=
std
::
rand
();
}
return
QString
::
number
(
r
);
}
src/MeasurementComplexItem/geometry/MeasurementArea.h
View file @
b098b99a
...
@@ -107,7 +107,7 @@ private:
...
@@ -107,7 +107,7 @@ private:
// Tile stuff.
// Tile stuff.
TilePtr
_tiles
;
TilePtr
_tiles
;
std
::
map
<
std
::
int64_t
/*id*/
,
int
>
_indexMap
;
std
::
map
<
QString
/*id*/
,
int
>
_indexMap
;
QTimer
_timer
;
QTimer
_timer
;
STATE
_state
;
STATE
_state
;
QFutureWatcher
<
TilePtr
>
_watcher
;
QFutureWatcher
<
TilePtr
>
_watcher
;
...
...
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
View file @
b098b99a
...
@@ -6,7 +6,7 @@ const char *MeasurementTile::settingsGroup = "MeasurementTile";
...
@@ -6,7 +6,7 @@ const char *MeasurementTile::settingsGroup = "MeasurementTile";
const
char
*
MeasurementTile
::
nameString
=
"MeasurementTile"
;
const
char
*
MeasurementTile
::
nameString
=
"MeasurementTile"
;
MeasurementTile
::
MeasurementTile
(
QObject
*
parent
)
MeasurementTile
::
MeasurementTile
(
QObject
*
parent
)
:
GeoArea
(
parent
),
_progress
(
0
),
_id
(
0
)
{
:
GeoArea
(
parent
),
_progress
(
0
),
_id
(
MeasurementTile
::
randomId
()
)
{
init
();
init
();
}
}
...
@@ -38,9 +38,9 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
...
@@ -38,9 +38,9 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
void
MeasurementTile
::
init
()
{
this
->
setObjectName
(
nameString
);
}
void
MeasurementTile
::
init
()
{
this
->
setObjectName
(
nameString
);
}
int64_t
MeasurementTile
::
id
()
const
{
return
_id
;
}
QString
MeasurementTile
::
id
()
const
{
return
_id
;
}
void
MeasurementTile
::
setId
(
const
int64_t
&
id
)
{
void
MeasurementTile
::
setId
(
const
QString
&
id
)
{
if
(
_id
!=
id
)
{
if
(
_id
!=
id
)
{
_id
=
id
;
_id
=
id
;
emit
idChanged
();
emit
idChanged
();
...
@@ -49,6 +49,19 @@ void MeasurementTile::setId(const int64_t &id) {
...
@@ -49,6 +49,19 @@ void MeasurementTile::setId(const int64_t &id) {
QList
<
QGeoCoordinate
>
MeasurementTile
::
tile
()
const
{
return
coordinateList
();
}
QList
<
QGeoCoordinate
>
MeasurementTile
::
tile
()
const
{
return
coordinateList
();
}
QString
MeasurementTile
::
randomId
(
int
length
)
{
static
const
QString
values
(
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789"
);
QString
str
;
std
::
srand
(
std
::
time
(
nullptr
));
for
(
int
i
=
0
;
i
<
length
;
++
i
)
{
int
index
=
std
::
rand
()
%
values
.
length
();
QChar
c
=
values
.
at
(
index
);
str
.
append
(
c
);
}
return
str
;
}
double
MeasurementTile
::
progress
()
const
{
return
_progress
;
}
double
MeasurementTile
::
progress
()
const
{
return
_progress
;
}
void
MeasurementTile
::
setProgress
(
double
progress
)
{
void
MeasurementTile
::
setProgress
(
double
progress
)
{
...
...
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
View file @
b098b99a
...
@@ -24,11 +24,13 @@ public:
...
@@ -24,11 +24,13 @@ public:
double
progress
()
const
;
double
progress
()
const
;
void
setProgress
(
double
progress
);
void
setProgress
(
double
progress
);
int64_t
id
()
const
;
QString
id
()
const
;
void
setId
(
const
int64_t
&
id
);
void
setId
(
const
QString
&
id
);
QList
<
QGeoCoordinate
>
tile
()
const
;
QList
<
QGeoCoordinate
>
tile
()
const
;
static
QString
randomId
(
int
length
=
10
);
// Static Variables
// Static Variables
static
const
char
*
settingsGroup
;
static
const
char
*
settingsGroup
;
static
const
char
*
nameString
;
static
const
char
*
nameString
;
...
@@ -40,5 +42,5 @@ signals:
...
@@ -40,5 +42,5 @@ signals:
private:
private:
void
init
();
void
init
();
double
_progress
;
double
_progress
;
int64_t
_id
;
QString
_id
;
};
};
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp
View file @
b098b99a
...
@@ -86,6 +86,8 @@ std::size_t TaskDispatcher::pendingTasks() {
...
@@ -86,6 +86,8 @@ std::size_t TaskDispatcher::pendingTasks() {
return
this
->
_taskQueue
.
size
()
+
(
_running
?
1
:
0
);
return
this
->
_taskQueue
.
size
()
+
(
_running
?
1
:
0
);
}
}
bool
TaskDispatcher
::
idle
()
{
return
this
->
pendingTasks
()
==
0
;
}
void
TaskDispatcher
::
run
()
{
void
TaskDispatcher
::
run
()
{
while
(
true
)
{
while
(
true
)
{
ULock
lk1
(
this
->
_mutex
);
ULock
lk1
(
this
->
_mutex
);
...
...
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h
View file @
b098b99a
...
@@ -53,6 +53,7 @@ public:
...
@@ -53,6 +53,7 @@ public:
bool
isRunning
();
bool
isRunning
();
std
::
size_t
pendingTasks
();
std
::
size_t
pendingTasks
();
bool
idle
();
protected:
protected:
void
run
();
void
run
();
...
...
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
View file @
b098b99a
...
@@ -16,6 +16,8 @@ static const char *rosTopics = "/rosapi/topics";
...
@@ -16,6 +16,8 @@ static const char *rosTopics = "/rosapi/topics";
static
constexpr
auto
pollIntervall
=
std
::
chrono
::
milliseconds
(
200
);
static
constexpr
auto
pollIntervall
=
std
::
chrono
::
milliseconds
(
200
);
Rosbridge
::
STATE
translate
(
RosbridgeImpl
::
STATE
s
);
Rosbridge
::
Rosbridge
(
const
QUrl
url
,
QObject
*
parent
)
Rosbridge
::
Rosbridge
(
const
QUrl
url
,
QObject
*
parent
)
:
QObject
(
parent
),
_url
(
url
),
_running
(
false
)
{
:
QObject
(
parent
),
_url
(
url
),
_running
(
false
)
{
static
std
::
once_flag
flag
;
static
std
::
once_flag
flag
;
...
@@ -34,6 +36,9 @@ void Rosbridge::start() {
...
@@ -34,6 +36,9 @@ void Rosbridge::start() {
_impl
=
new
RosbridgeImpl
(
_url
);
_impl
=
new
RosbridgeImpl
(
_url
);
_impl
->
moveToThread
(
_t
);
_impl
->
moveToThread
(
_t
);
connect
(
_impl
,
&
RosbridgeImpl
::
stateChanged
,
this
,
&
Rosbridge
::
_onImplStateChanged
);
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
);
...
@@ -77,20 +82,7 @@ void Rosbridge::stop() {
...
@@ -77,20 +82,7 @@ void Rosbridge::stop() {
Rosbridge
::
STATE
Rosbridge
::
state
()
{
Rosbridge
::
STATE
Rosbridge
::
state
()
{
if
(
_running
)
{
if
(
_running
)
{
switch
(
_impl
->
state
())
{
return
translate
(
_impl
->
state
());
case
RosbridgeImpl
:
:
STATE
::
STOPPED
:
case
RosbridgeImpl
:
:
STATE
::
STOPPING
:
return
STATE
::
STOPPED
;
case
RosbridgeImpl
:
:
STATE
::
STARTING
:
return
STATE
::
STARTED
;
case
RosbridgeImpl
:
:
STATE
::
CONNECTED
:
return
STATE
::
CONNECTED
;
case
RosbridgeImpl
:
:
STATE
::
TIMEOUT
:
return
STATE
::
TIMEOUT
;
break
;
}
qCritical
()
<<
"unreachable branch!"
;
return
STATE
::
STOPPED
;
}
else
{
}
else
{
return
STATE
::
STOPPED
;
return
STATE
::
STOPPED
;
}
}
...
@@ -251,3 +243,30 @@ void Rosbridge::waitForService(const QString &service) {
...
@@ -251,3 +243,30 @@ void Rosbridge::waitForService(const QString &service) {
qDebug
()
<<
"waitForService: Rosbridge not connected."
;
qDebug
()
<<
"waitForService: Rosbridge not connected."
;
}
}
}
}
void
Rosbridge
::
_onImplStateChanged
()
{
static
STATE
oldState
=
STATE
::
STOPPED
;
auto
newState
=
translate
(
_impl
->
state
());
if
(
oldState
!=
newState
)
{
emit
stateChanged
();
}
oldState
=
newState
;
}
Rosbridge
::
STATE
translate
(
RosbridgeImpl
::
STATE
s
)
{
switch
(
s
)
{
case
RosbridgeImpl
:
:
STATE
::
STOPPED
:
case
RosbridgeImpl
:
:
STATE
::
STOPPING
:
return
Rosbridge
::
STATE
::
STOPPED
;
case
RosbridgeImpl
:
:
STATE
::
STARTING
:
return
Rosbridge
::
STATE
::
STARTED
;
case
RosbridgeImpl
:
:
STATE
::
CONNECTED
:
return
Rosbridge
::
STATE
::
CONNECTED
;
case
RosbridgeImpl
:
:
STATE
::
TIMEOUT
:
return
Rosbridge
::
STATE
::
TIMEOUT
;
break
;
}
qCritical
()
<<
"unreachable branch!"
;
return
Rosbridge
::
STATE
::
STOPPED
;
}
src/MeasurementComplexItem/rosbridge/rosbridge.h
View file @
b098b99a
...
@@ -101,6 +101,9 @@ signals:
...
@@ -101,6 +101,9 @@ signals:
void
_unadvertiseAllServices
();
void
_unadvertiseAllServices
();
private:
private:
void
_onImplStateChanged
();
std
::
atomic
<
STATE
>
_state
;
RosbridgeImpl
*
_impl
;
RosbridgeImpl
*
_impl
;
QThread
*
_t
;
QThread
*
_t
;
QUrl
_url
;
QUrl
_url
;
...
...
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp
View file @
b098b99a
...
@@ -114,7 +114,6 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
...
@@ -114,7 +114,6 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
auto
it
=
_advertisedTopics
.
find
(
topic
);
auto
it
=
_advertisedTopics
.
find
(
topic
);
if
(
Q_LIKELY
(
it
!=
_advertisedTopics
.
end
()))
{
if
(
Q_LIKELY
(
it
!=
_advertisedTopics
.
end
()))
{
_advertisedTopics
.
erase
(
it
);
QJsonObject
o
;
QJsonObject
o
;
o
[
opKey
]
=
unadvertiseOpKey
;
o
[
opKey
]
=
unadvertiseOpKey
;
...
@@ -123,6 +122,8 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
...
@@ -123,6 +122,8 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
QString
payload
=
QString
payload
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
_webSocket
.
sendTextMessage
(
payload
);
_webSocket
.
sendTextMessage
(
payload
);
_advertisedTopics
.
erase
(
it
);
}
else
{
}
else
{
qDebug
()
<<
"Topic "
<<
topic
<<
" not advertised."
;
qDebug
()
<<
"Topic "
<<
topic
<<
" not advertised."
;
}
}
...
@@ -170,7 +171,6 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
...
@@ -170,7 +171,6 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
auto
it
=
_subscribedTopics
.
find
(
topic
);
auto
it
=
_subscribedTopics
.
find
(
topic
);
if
(
Q_LIKELY
(
it
!=
_subscribedTopics
.
end
()))
{
if
(
Q_LIKELY
(
it
!=
_subscribedTopics
.
end
()))
{
_subscribedTopics
.
erase
(
it
);
QJsonObject
o
;
QJsonObject
o
;
o
[
opKey
]
=
unsubscribeOpKey
;
o
[
opKey
]
=
unsubscribeOpKey
;
...
@@ -179,6 +179,8 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
...
@@ -179,6 +179,8 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
QString
payload
=
QString
payload
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
_webSocket
.
sendTextMessage
(
payload
);
_webSocket
.
sendTextMessage
(
payload
);
_subscribedTopics
.
erase
(
it
);
}
else
{
}
else
{
qDebug
()
<<
"unsubscribeTopic: topic "
<<
topic
<<
" already subscribed!"
;
qDebug
()
<<
"unsubscribeTopic: topic "
<<
topic
<<
" already subscribed!"
;
}
}
...
@@ -293,7 +295,6 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
...
@@ -293,7 +295,6 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
auto
it
=
_advertisedServices
.
find
(
service
);
auto
it
=
_advertisedServices
.
find
(
service
);
if
(
it
!=
_advertisedServices
.
end
())
{
if
(
it
!=
_advertisedServices
.
end
())
{
it
=
_advertisedServices
.
erase
(
it
);
QJsonObject
o
;
QJsonObject
o
;
o
[
opKey
]
=
unadvertiseServiceOpKey
;
o
[
opKey
]
=
unadvertiseServiceOpKey
;
...
@@ -302,6 +303,8 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
...
@@ -302,6 +303,8 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
QString
payload
=
QString
payload
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
_webSocket
.
sendTextMessage
(
payload
);
_webSocket
.
sendTextMessage
(
payload
);
it
=
_advertisedServices
.
erase
(
it
);
}
else
{
}
else
{
qDebug
()
<<
"unadvertiseService: Service "
<<
service
qDebug
()
<<
"unadvertiseService: Service "
<<
service
<<
" not advertised."
;
<<
" not advertised."
;
...
@@ -369,6 +372,7 @@ void RosbridgeImpl::_doAction() {
...
@@ -369,6 +372,7 @@ void RosbridgeImpl::_doAction() {
}
}
void
RosbridgeImpl
::
_onTextMessageReceived
(
const
QString
&
message
)
{
void
RosbridgeImpl
::
_onTextMessageReceived
(
const
QString
&
message
)
{
qDebug
()
<<
"_onTextMessageReceived: "
<<
message
;
QJsonParseError
e
;
QJsonParseError
e
;
auto
d
=
QJsonDocument
::
fromJson
(
message
.
toUtf8
(),
&
e
);
auto
d
=
QJsonDocument
::
fromJson
(
message
.
toUtf8
(),
&
e
);
if
(
!
d
.
isNull
())
{
if
(
!
d
.
isNull
())
{
...
...
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
View file @
b098b99a
...
@@ -3,147 +3,140 @@
...
@@ -3,147 +3,140 @@
#include <string>
#include <string>
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QJsonObject>
#include <QJsonValue>
namespace
ros_bridge
{
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
//! @brief Namespace containing classes and methodes for geometry_msgs
//! generation.
namespace
geographic_msgs
{
namespace
geographic_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message
//! generation.
namespace
geo_point
{
namespace
geo_point
{
std
::
string
messageType
();
std
::
string
messageType
();
namespace
{
const
char
*
lonKey
=
"longitude"
;
const
char
*
latKey
=
"latitude"
;
const
char
*
altKey
=
"altitude"
;
}
// namespace
//! @brief C++ representation of geographic_msgs/GeoPoint.
//! @brief C++ representation of geographic_msgs/GeoPoint.
template
<
class
FloatType
=
_Float64
,
class
OStream
=
std
::
ostream
>
template
<
class
FloatType
=
_Float64
,
class
OStream
=
std
::
ostream
>
class
GenericGeoPoint
{
class
GenericGeoPoint
{
public:
public:
GenericGeoPoint
()
GenericGeoPoint
()
:
_latitude
(
0
),
_longitude
(
0
),
_altitude
(
0
)
{}
:
_latitude
(
0
)
GenericGeoPoint
(
const
GenericGeoPoint
&
other
)
,
_longitude
(
0
)
:
_latitude
(
other
.
latitude
()),
_longitude
(
other
.
longitude
()),
,
_altitude
(
0
)
_altitude
(
other
.
altitude
())
{}
{}
GenericGeoPoint
(
FloatType
latitude
,
FloatType
longitude
,
FloatType
altitude
)
GenericGeoPoint
(
const
GenericGeoPoint
&
other
)
:
_latitude
(
latitude
),
_longitude
(
longitude
),
_altitude
(
altitude
)
{}
:
_latitude
(
other
.
latitude
())
,
_longitude
(
other
.
longitude
())
FloatType
latitude
()
const
{
return
_latitude
;
}
,
_altitude
(
other
.
altitude
())
FloatType
longitude
()
const
{
return
_longitude
;
}
{}
FloatType
altitude
()
const
{
return
_altitude
;
}
GenericGeoPoint
(
FloatType
latitude
,
FloatType
longitude
,
FloatType
altitude
)
:
_latitude
(
latitude
)
void
setLatitude
(
FloatType
latitude
)
{
_latitude
=
latitude
;
}
,
_longitude
(
longitude
)
void
setLongitude
(
FloatType
longitude
)
{
_longitude
=
longitude
;
}
,
_altitude
(
altitude
)
void
setAltitude
(
FloatType
altitude
)
{
_altitude
=
altitude
;
}
{}
bool
operator
==
(
const
GenericGeoPoint
&
p1
)
{
FloatType
latitude
()
const
{
return
_latitude
;}
return
(
p1
.
_latitude
==
this
->
_latitude
&&
FloatType
longitude
()
const
{
return
_longitude
;}
p1
.
_longitude
==
this
->
_longitude
&&
FloatType
altitude
()
const
{
return
_altitude
;}
p1
.
_altitude
==
this
->
_altitude
);
}
void
setLatitude
(
FloatType
latitude
)
{
_latitude
=
latitude
;}
void
setLongitude
(
FloatType
longitude
)
{
_longitude
=
longitude
;}
bool
operator
!=
(
const
GenericGeoPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
void
setAltitude
(
FloatType
altitude
)
{
_altitude
=
altitude
;}
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericGeoPoint
&
p
)
{
bool
operator
==
(
const
GenericGeoPoint
&
p1
)
{
os
<<
"lat: "
<<
p
.
_latitude
<<
" lon: "
<<
p
.
_longitude
return
(
p1
.
_latitude
==
this
->
_latitude
<<
" alt: "
<<
p
.
_altitude
;
&&
p1
.
_longitude
==
this
->
_longitude
return
os
;
&&
p1
.
_altitude
==
this
->
_altitude
);
}
}
bool
operator
!=
(
const
GenericGeoPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericGeoPoint
&
p
)
{
os
<<
"lat: "
<<
p
.
_latitude
<<
" lon: "
<<
p
.
_longitude
<<
" alt: "
<<
p
.
_altitude
;
return
os
;
}
private:
private:
FloatType
_latitude
;
FloatType
_latitude
;
FloatType
_longitude
;
FloatType
_longitude
;
FloatType
_altitude
;
FloatType
_altitude
;
};
};
typedef
GenericGeoPoint
<>
GeoPoint
;
typedef
GenericGeoPoint
<>
GeoPoint
;
namespace
detail
{
namespace
detail
{
template
<
class
T
>
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
{
return
p
.
altitude
();
return
p
.
altitude
();
}
}
template
<
class
T
>
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
(
void
)
p
;
{
return
0
.
0
;
(
void
)
p
;
}
return
0
.
0
;
}
template
<
class
T
>
void
setAltitude
(
const
QJsonValue
&
o
,
T
&
p
,
template
<
class
T
>
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
p
.
setAltitude
(
o
.
toDouble
());
{
}
p
.
setAltitude
(
doc
.
GetFloat
());
}
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
p
;
}
}
// namespace detail
template
<
class
T
>
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
void
setAltitude
(
const
QJsonValue
&
o
,
T
&
p
,
{
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
value
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
(
void
)
o
;
value
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
(
void
)
p
;
typedef
typename
traits
::
Select
<
traits
::
HasMemberAltitude
<
T
>::
value
,
traits
::
Has3Components
,
traits
::
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
auto
altitude
=
detail
::
getAltitude
(
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude() replace it by 0.0;
value
.
AddMember
(
"altitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
altitude
),
allocator
);
return
true
;
}
}
}
// namespace detail
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
QJsonObject
&
value
)
{
value
[
latKey
]
=
p
.
latitude
();
value
[
lonKey
]
=
p
.
longitude
();
typedef
typename
traits
::
Select
<
traits
::
HasMemberAltitude
<
T
>::
value
,
traits
::
Has3Components
,
traits
::
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
auto
altitude
=
detail
::
getAltitude
(
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude()
// replace it by 0.0;
value
[
altKey
]
=
altitude
;
return
true
;
}
template
<
class
PointType
>
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
bool
fromJson
(
const
QJsonObject
&
value
,
PointType
&
p
)
{
PointType
&
p
)
{
if
(
!
value
.
contains
(
"latitude"
)
||
!
value
[
"latitude"
].
isDouble
())
{
if
(
!
value
.
HasMember
(
"latitude"
)
||
!
value
[
"latitude"
].
IsFloat
()){
return
false
;
assert
(
false
);
}
return
false
;
if
(
!
value
.
contains
(
"longitude"
)
||
!
value
[
"longitude"
].
isDouble
())
{
}
return
false
;
if
(
!
value
.
HasMember
(
"longitude"
)
||
!
value
[
"longitude"
].
IsFloat
()){
}
assert
(
false
);
if
(
!
value
.
contains
(
"altitude"
)
||
!
value
[
"altitude"
].
isDouble
())
{
return
false
;
return
false
;
}
}
if
(
!
value
.
HasMember
(
"altitude"
)
||
!
value
[
"altitude"
].
IsFloat
()){
assert
(
false
);
p
.
setLatitude
(
value
[
"latitude"
].
toDouble
());
return
false
;
p
.
setLongitude
(
value
[
"longitude"
].
toDouble
());
}
typedef
typename
traits
::
Select
<
traits
::
HasMemberSetAltitude
<
PointType
>::
value
,
p
.
setLatitude
(
value
[
"latitude"
].
GetFloat
());
traits
::
Has3Components
,
p
.
setLongitude
(
value
[
"longitude"
].
GetFloat
());
traits
::
Has2Components
>::
Result
typedef
typename
traits
::
Select
<
Components
;
// Check if PointType has 2 or 3 dimensions.
traits
::
HasMemberSetAltitude
<
PointType
>::
value
,
detail
::
setAltitude
(
traits
::
Has3Components
,
value
[
"altitude"
],
p
,
traits
::
Has2Components
>::
Result
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude() discard
Components
;
// Check if PointType has 2 or 3 dimensions.
// doc["altitude"];
detail
::
setAltitude
(
value
[
"altitude"
],
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude() discard doc["altitude"];
return
true
;
return
true
;
}
// namespace detail
}
// namespace detail
}
// namespace geo_point
}
// namespace geopoint
}
// namespace geographic_msgs
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace messages
}
// namespace ros_bridge
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp
deleted
100644 → 0
View file @
9e620065
#include "point32.h"
std
::
string
ros_bridge
::
messages
::
geometry_msgs
::
point32
::
messageType
(){
return
"geometry_msgs/Point32"
;
}
src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
deleted
100644 → 0
View file @
9e620065
#pragma once
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
geometry_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
namespace
point32
{
std
::
string
messageType
();
namespace
detail
{
using
namespace
ros_bridge
::
traits
;
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has3Components
>
)
{
return
p
.
z
();
}
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
// p has no member z() -> add 0.
}
template
<
class
T
,
typename
V
>
bool
setZ
(
const
rapidjson
::
Value
&
doc
,
const
T
&
p
,
Type2Type
<
V
>
)
{
p
.
setZ
(
doc
[
"z"
].
GetFloat
());
return
true
;
}
template
<
class
T
>
bool
setZ
(
const
rapidjson
::
Value
&
doc
,
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
p
;
return
true
;
}
}
// namespace detail
//! @brief C++ representation of a geometry_msgs/Point/Point32
template
<
typename
FloatType
=
_Float64
,
class
OStream
=
std
::
ostream
>
class
GenericPoint
{
public:
GenericPoint
(
FloatType
x
,
FloatType
y
,
FloatType
z
)
:
_x
(
x
),
_y
(
y
),
_z
(
z
){}
FloatType
x
()
const
{
return
_x
;}
FloatType
y
()
const
{
return
_y
;}
FloatType
z
()
const
{
return
_z
;}
void
setX
(
FloatType
x
)
{
_x
=
x
;}
void
setY
(
FloatType
y
)
{
_y
=
y
;}
void
setZ
(
FloatType
z
)
{
_z
=
z
;}
bool
operator
==
(
const
GenericPoint
&
p1
)
{
return
(
p1
.
_x
==
this
->
_x
&&
p1
.
_y
==
this
->
_y
&&
p1
.
_z
==
this
->
_z
);
}
bool
operator
!=
(
const
GenericPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericPoint
&
p
)
{
os
<<
"x: "
<<
p
.
_x
<<
" y: "
<<
p
.
_y
<<
" z: "
<<
p
.
_z
;
return
os
;
}
private:
FloatType
_x
;
FloatType
_y
;
FloatType
_z
;
};
typedef
GenericPoint
<>
Point
;
typedef
GenericPoint
<
_Float32
>
Point32
;
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
ros_bridge
::
traits
;
value
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
(
p
.
x
()),
allocator
);
value
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
(
p
.
y
()),
allocator
);
typedef
typename
Select
<
HasMemberZ
<
T
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
auto
z
=
detail
::
getZ
(
p
,
Type2Type
<
Components
>
());
// If T has no member z() replace it by 0.
value
.
AddMember
(
"z"
,
rapidjson
::
Value
().
SetFloat
(
z
),
allocator
);
return
true
;
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
using
namespace
ros_bridge
::
traits
;
if
(
!
value
.
HasMember
(
"x"
)
||
!
value
[
"x"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"y"
)
||
!
value
[
"y"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"z"
)
||
!
value
[
"z"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setX
(
value
[
"x"
].
GetFloat
());
p
.
setY
(
value
[
"y"
].
GetFloat
());
typedef
typename
Select
<
HasMemberSetZ
<
PointType
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
(
void
)
detail
::
setZ
(
value
[
"z"
],
p
,
Type2Type
<
Components
>
());
// If PointType has no member z() discard doc["z"].
return
true
;
}
}
// namespace point32
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp
deleted
100644 → 0
View file @
9e620065
#include "polygon.h"
std
::
string
ros_bridge
::
messages
::
geometry_msgs
::
polygon
::
messageType
(){
return
"geometry_msgs/Polygon"
;
}
src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
deleted
100644 → 0
View file @
9e620065
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/point32.h"
#include <type_traits>
#include <vector>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
geometry_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
namespace
polygon
{
std
::
string
messageType
();
//! @brief C++ representation of geometry_msgs/Polygon
template
<
class
PointTypeCVR
,
template
<
class
,
class
...
>
class
ContainerType
=
std
::
vector
>
class
GenericPolygon
{
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
PointTypeCVR
>>
PointType
;
public:
GenericPolygon
()
{}
GenericPolygon
(
const
GenericPolygon
&
poly
)
:
_points
(
poly
.
points
()){}
const
ContainerType
<
PointType
>
&
points
()
const
{
return
_points
;}
ContainerType
<
PointType
>
&
points
()
{
return
_points
;}
GenericPolygon
&
operator
=
(
const
GenericPolygon
&
other
)
{
this
->
_points
=
other
.
_points
;
return
*
this
;
}
private:
ContainerType
<
PointType
>
_points
;
};
typedef
GenericPolygon
<
geometry_msgs
::
point32
::
Point
>
Polygon
;
template
<
class
PolygonType
>
bool
toJson
(
const
PolygonType
&
poly
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
geometry_msgs
::
point32
;
rapidjson
::
Value
points
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
std
::
uint64_t
(
poly
.
points
().
size
());
++
i
)
{
rapidjson
::
Document
point
(
rapidjson
::
kObjectType
);
if
(
!
point32
::
toJson
(
poly
.
points
()[
i
],
point
,
allocator
)
){
assert
(
false
);
return
false
;
}
points
.
PushBack
(
point
,
allocator
);
}
value
.
AddMember
(
"points"
,
points
,
allocator
);
return
true
;
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonType
&
poly
)
{
using
namespace
geometry_msgs
::
point32
;
if
(
!
value
.
HasMember
(
"points"
)
||
!
value
[
"points"
].
IsArray
()){
assert
(
false
);
return
false
;
}
const
auto
&
jsonArray
=
value
[
"points"
].
GetArray
();
poly
.
points
().
clear
();
poly
.
points
().
reserve
(
jsonArray
.
Size
());
typedef
decltype
(
poly
.
points
()[
0
])
PointTypeCVR
;
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
PointTypeCVR
>>
PointType
;
for
(
long
i
=
0
;
i
<
jsonArray
.
Size
();
++
i
){
PointType
pt
;
if
(
!
point32
::
fromJson
(
jsonArray
[
i
],
pt
)
){
assert
(
false
);
return
false
;
}
poly
.
points
().
push_back
(
std
::
move
(
pt
));
}
return
true
;
}
}
// namespace polygon
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp
deleted
100644 → 0
View file @
9e620065
#include "polygon_stamped.h"
std
::
string
ros_bridge
::
messages
::
geometry_msgs
::
polygon_stamped
::
messageType
(){
return
"geometry_msgs/PolygonStamped"
;
}
src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
deleted
100644 → 0
View file @
9e620065
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/polygon.h"
#include "ros_bridge/include/messages/std_msgs/header.h"
#include "ros_bridge/include/message_traits.h"
#include <type_traits>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
geometry_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation.
namespace
polygon_stamped
{
std
::
string
messageType
();
//! @brief C++ representation of geometry_msgs/PolygonStamped
template
<
class
PolygonType
=
geometry_msgs
::
polygon
::
Polygon
,
class
HeaderType
=
std_msgs
::
header
::
Header
>
class
GenericPolygonStamped
{
typedef
PolygonType
Polygon
;
public:
GenericPolygonStamped
(){}
GenericPolygonStamped
(
const
GenericPolygonStamped
&
other
)
:
_header
(
other
.
header
())
,
_polygon
(
other
.
polygon
())
{}
GenericPolygonStamped
(
const
HeaderType
&
header
,
Polygon
&
polygon
)
:
_header
(
header
)
,
_polygon
(
polygon
)
{}
const
HeaderType
&
header
()
const
{
return
_header
;}
const
Polygon
&
polygon
()
const
{
return
_polygon
;}
HeaderType
&
header
()
{
return
_header
;}
Polygon
&
polygon
()
{
return
_polygon
;}
private:
HeaderType
_header
;
Polygon
_polygon
;
};
typedef
GenericPolygonStamped
<>
PolygonStamped
;
// ===================================================================================
namespace
detail
{
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
traits
::
Int2Type
<
1
>
)
{
// polyStamped.header() exists
using
namespace
std_msgs
;
typedef
decltype
(
polyStamped
.
header
())
HeaderTypeCVR
;
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
HeaderTypeCVR
>>
HeaderType
;
HeaderType
header
;
bool
ret
=
header
::
fromJson
(
doc
,
header
);
polyStamped
.
header
()
=
header
;
return
ret
;
}
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
traits
::
Int2Type
<
0
>
)
{
// polyStamped.header() does not exists
(
void
)
doc
;
(
void
)
polyStamped
;
return
true
;
}
template
<
class
PolygonType
,
class
HeaderType
>
bool
_toJson
(
const
PolygonType
&
poly
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
std_msgs
;
using
namespace
geometry_msgs
;
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
header
::
toJson
(
h
,
header
,
allocator
)){
assert
(
false
);
return
false
;
}
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
polygon
::
toJson
(
poly
,
polygon
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"header"
,
header
,
allocator
);
value
.
AddMember
(
"polygon"
,
polygon
,
allocator
);
return
true
;
}
template
<
class
PolyStamped
,
int
k
>
bool
_toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
k
>
){
// PolyStamped has member header(), use integraded header.
return
_toJson
(
polyStamped
,
polyStamped
.
header
(),
value
,
allocator
);
}
template
<
class
PolyStamped
>
bool
_toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
0
>
){
// PolyStamped has no member header(), generate one on the fly.
using
namespace
std_msgs
::
header
;
return
_toJson
(
polyStamped
,
Header
(),
value
,
allocator
);
}
}
// namespace detail
// ===================================================================================
template
<
class
PolygonType
,
class
HeaderType
>
bool
toJson
(
const
PolygonType
&
poly
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
return
detail
::
_toJson
(
poly
,
h
,
value
,
allocator
);
}
template
<
class
PolyStamped
>
bool
toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
typedef
traits
::
HasMemberHeader
<
PolyStamped
>
HasHeader
;
return
detail
::
_toJson
(
polyStamped
,
value
,
allocator
,
traits
::
Int2Type
<
HasHeader
::
value
>
());
}
template
<
class
PolygonType
,
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonType
&
polyStamped
)
{
using
namespace
geometry_msgs
::
polygon
;
if
(
!
value
.
HasMember
(
"header"
)
){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"polygon"
)
){
assert
(
false
);
return
false
;
}
typedef
traits
::
HasMemberSetHeader
<
PolygonType
>
HasHeader
;
if
(
!
detail
::
setHeader
(
value
[
"header"
],
polyStamped
,
traits
::
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
if
(
!
polygon
::
fromJson
(
value
[
"polygon"
],
polyStamped
.
polygon
())
){
assert
(
false
);
return
false
;
}
return
true
;
}
}
// namespace polygon_stamped
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
View file @
b098b99a
#pragma once
#pragma once
#include
"ros_bridge/rapidjson/include/rapidjson/document.h"
#include
<QJsonObject>
namespace
ros_bridge
{
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
//! @brief Namespace containing classes and methodes ros message generation.
...
@@ -32,19 +32,18 @@ protected:
...
@@ -32,19 +32,18 @@ protected:
};
};
template
<
class
HeartbeatType
>
template
<
class
HeartbeatType
>
bool
toJson
(
const
HeartbeatType
&
heartbeat
,
rapidjson
::
Value
&
value
,
bool
toJson
(
const
HeartbeatType
&
heartbeat
,
QJsonObject
&
value
)
{
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
[
"status"
]
=
heartbeat
.
status
();
value
.
AddMember
(
"status"
,
std
::
int32_t
(
heartbeat
.
status
()),
allocator
);
return
true
;
return
true
;
}
}
template
<
class
HeartbeatType
>
template
<
class
HeartbeatType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
HeartbeatType
&
heartbeat
)
{
bool
fromJson
(
const
QJsonObject
&
value
,
HeartbeatType
&
heartbeat
)
{
if
(
!
value
.
HasMember
(
"status"
)
||
!
value
[
"status"
].
IsInt
())
{
if
(
!
value
.
contains
(
"status"
)
||
!
value
[
"status"
].
isDouble
())
{
return
false
;
return
false
;
}
}
heartbeat
.
setStatus
(
value
[
"status"
].
Get
Int
());
heartbeat
.
setStatus
(
value
[
"status"
].
to
Int
());
return
true
;
return
true
;
}
}
...
...
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp
View file @
b098b99a
#include "labeled_progress.h"
#include "labeled_progress.h"
std
::
s
tring
ros_bridge
::
messages
::
nemo_msgs
::
labeled_progress
::
messageType
()
{
QS
tring
ros_bridge
::
messages
::
nemo_msgs
::
labeled_progress
::
messageType
()
{
return
"nemo_msgs/LabeledProgress"
;
return
"nemo_msgs/LabeledProgress"
;
}
}
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h
View file @
b098b99a
#pragma once
#pragma once
#include
"ros_bridge/rapidjson/include/rapidjson/document.h"
#include
<QJsonObject>
#include <vector>
#include <QDebug>
#include <QString>
namespace
ros_bridge
{
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
//! @brief Namespace containing classes and methodes ros message generation.
...
@@ -14,7 +15,7 @@ namespace nemo_msgs {
...
@@ -14,7 +15,7 @@ namespace nemo_msgs {
//! generation.
//! generation.
namespace
labeled_progress
{
namespace
labeled_progress
{
std
::
s
tring
messageType
();
QS
tring
messageType
();
namespace
{
namespace
{
const
char
*
progressKey
=
"progress"
;
const
char
*
progressKey
=
"progress"
;
...
@@ -24,40 +25,39 @@ const char *idKey = "id";
...
@@ -24,40 +25,39 @@ const char *idKey = "id";
//! @brief C++ representation of nemo_msgs/labeled_progress message
//! @brief C++ representation of nemo_msgs/labeled_progress message
class
LabeledProgress
{
class
LabeledProgress
{
public:
public:
LabeledProgress
()
:
_id
(
0
),
_progress
(
0
)
{}
LabeledProgress
()
:
_id
(
""
),
_progress
(
0
)
{}
LabeledProgress
(
double
progress
,
lo
ng
id
)
:
_id
(
id
),
_progress
(
progress
)
{}
LabeledProgress
(
double
progress
,
QStri
ng
id
)
:
_id
(
id
),
_progress
(
progress
)
{}
lo
ng
id
()
const
{
return
_id
;
}
QStri
ng
id
()
const
{
return
_id
;
}
void
setId
(
lo
ng
id
)
{
_id
=
id
;
}
void
setId
(
QStri
ng
id
)
{
_id
=
id
;
}
double
progress
()
const
{
return
_progress
;
}
double
progress
()
const
{
return
_progress
;
}
void
setProgress
(
double
progress
)
{
_progress
=
progress
;
}
void
setProgress
(
double
progress
)
{
_progress
=
progress
;
}
protected:
protected:
lo
ng
_id
;
QStri
ng
_id
;
double
_progress
;
double
_progress
;
};
};
template
<
class
LabeledProgressType
>
template
<
class
LabeledProgressType
>
bool
toJson
(
const
LabeledProgressType
&
lp
,
rapidjson
::
Value
&
value
,
bool
toJson
(
const
LabeledProgressType
&
lp
,
QJsonObject
&
value
)
{
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
[
idKey
]
=
lp
.
id
();
value
.
AddMember
(
idKey
,
lp
.
id
(),
allocator
);
value
[
progressKey
]
=
lp
.
progress
();
value
.
AddMember
(
progressKey
,
lp
.
progress
(),
allocator
);
return
true
;
return
true
;
}
}
template
<
class
ProgressType
>
template
<
class
ProgressType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
ProgressType
&
p
)
{
bool
fromJson
(
const
QJsonObject
&
value
,
ProgressType
&
p
)
{
if
(
!
value
.
HasMember
(
progressKey
)
||
!
value
[
progressKey
].
I
sDouble
())
{
if
(
!
value
.
contains
(
progressKey
)
||
!
value
[
progressKey
].
i
sDouble
())
{
return
false
;
return
false
;
}
}
if
(
!
value
.
HasMember
(
idKey
)
||
!
value
[
idKey
].
IsInt64
())
{
if
(
!
value
.
contains
(
idKey
)
||
!
value
[
idKey
].
isString
())
{
return
false
;
return
false
;
}
}
p
.
setId
(
value
[
idKey
].
GetInt64
());
p
.
setId
(
value
[
idKey
].
toString
());
p
.
setProgress
(
value
[
progressKey
].
Get
Double
());
p
.
setProgress
(
value
[
progressKey
].
to
Double
());
return
true
;
return
true
;
}
}
...
...
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h
View file @
b098b99a
...
@@ -4,8 +4,9 @@
...
@@ -4,8 +4,9 @@
#pragma once
#pragma once
#include "labeled_progress.h"
#include "labeled_progress.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QJsonArray>
#include <QJsonObject>
#include <QVector>
#include <QVector>
namespace
ros_bridge
{
namespace
ros_bridge
{
...
@@ -43,34 +44,33 @@ protected:
...
@@ -43,34 +44,33 @@ protected:
typedef
GenericProgressArray
<>
ProgressArray
;
typedef
GenericProgressArray
<>
ProgressArray
;
template
<
class
Array
>
template
<
class
Array
>
bool
toJson
(
const
Array
&
array
,
QJsonObject
&
value
)
{
bool
toJson
(
const
Array
&
array
,
rapidjson
::
Value
&
value
,
QJsonArray
jsonArray
;
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Value
jsonArray
(
rapidjson
::
kArrayType
);
for
(
const
auto
&
lp
:
array
.
progress_array
())
{
for
(
const
auto
&
lp
:
array
.
progress_array
())
{
rapidjson
::
Value
jsonLp
;
QJsonObject
jsonLp
;
if
(
labeled_progress
::
toJson
(
lp
,
jsonLp
,
allocator
))
{
if
(
labeled_progress
::
toJson
(
lp
,
jsonLp
))
{
jsonArray
.
PushBack
(
jsonLp
,
allocator
);
jsonArray
.
append
(
std
::
move
(
jsonLp
)
);
}
else
{
}
else
{
return
false
;
return
false
;
}
}
}
}
value
.
AddMember
(
rapidjson
::
Value
(
progressArrayKey
),
jsonArray
,
allocator
);
value
[
progressArrayKey
]
=
std
::
move
(
jsonArray
);
return
true
;
return
true
;
}
}
template
<
class
Array
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
Array
&
p
)
{
template
<
class
Array
>
bool
fromJson
(
const
QJsonObject
&
value
,
Array
&
p
)
{
if
(
!
value
.
HasMember
(
progressArrayKey
)
||
if
(
!
value
.
contains
(
progressArrayKey
)
||
!
value
[
progressArrayKey
].
isArray
())
{
!
value
[
progressArrayKey
].
IsArray
())
{
return
false
;
return
false
;
}
}
const
auto
&
jsonArray
=
value
[
progressArrayKey
].
Get
Array
();
const
auto
jsonArray
=
value
[
progressArrayKey
].
to
Array
();
p
.
progress_array
().
clear
();
p
.
progress_array
().
clear
();
p
.
progress_array
().
reserve
(
jsonArray
.
S
ize
());
p
.
progress_array
().
reserve
(
jsonArray
.
s
ize
());
for
(
long
i
=
0
;
i
<
jsonArray
.
S
ize
();
++
i
)
{
for
(
long
i
=
0
;
i
<
jsonArray
.
s
ize
();
++
i
)
{
labeled_progress
::
LabeledProgress
lp
;
labeled_progress
::
LabeledProgress
lp
;
if
(
!
labeled_progress
::
fromJson
(
jsonArray
[
i
],
lp
))
{
if
(
!
jsonArray
[
i
].
isObject
()
||
!
labeled_progress
::
fromJson
(
jsonArray
[
i
].
toObject
(),
lp
))
{
return
false
;
return
false
;
}
else
{
}
else
{
p
.
progress_array
().
push_back
(
lp
);
p
.
progress_array
().
push_back
(
lp
);
...
...
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp
View file @
b098b99a
#include "tile.h"
#include "tile.h"
std
::
s
tring
ros_bridge
::
messages
::
nemo_msgs
::
tile
::
messageType
()
{
QS
tring
ros_bridge
::
messages
::
nemo_msgs
::
tile
::
messageType
()
{
return
"nemo_msgs/Tile"
;
return
"nemo_msgs/Tile"
;
}
}
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h
View file @
b098b99a
#pragma once
#pragma once
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <vector>
#include <vector>
#include <QJsonArray>
#include <QString>
namespace
ros_bridge
{
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
namespace
messages
{
...
@@ -15,7 +17,7 @@ namespace nemo_msgs {
...
@@ -15,7 +17,7 @@ namespace nemo_msgs {
//! generation.
//! generation.
namespace
tile
{
namespace
tile
{
std
::
s
tring
messageType
();
QS
tring
messageType
();
namespace
{
namespace
{
const
char
*
progressKey
=
"progress"
;
const
char
*
progressKey
=
"progress"
;
...
@@ -30,11 +32,11 @@ class GenericTile {
...
@@ -30,11 +32,11 @@ class GenericTile {
public:
public:
typedef
Container
<
GeoPoint
>
GeoContainer
;
typedef
Container
<
GeoPoint
>
GeoContainer
;
GenericTile
()
{}
GenericTile
()
{}
GenericTile
(
const
GeoContainer
&
tile
,
double
progress
,
long
id
)
GenericTile
(
const
GeoContainer
&
tile
,
double
progress
,
const
QString
&
id
)
:
_tile
(
tile
),
_id
(
id
),
_progress
(
progress
)
{}
:
_tile
(
tile
),
_id
(
id
),
_progress
(
progress
)
{}
lo
ng
id
()
const
{
return
_id
;
}
QStri
ng
id
()
const
{
return
_id
;
}
void
setId
(
long
id
)
{
_id
=
id
;
}
void
setId
(
const
QString
&
id
)
{
_id
=
id
;
}
double
progress
()
const
{
return
_progress
;
}
double
progress
()
const
{
return
_progress
;
}
void
setProgress
(
double
progress
)
{
_progress
=
progress
;
}
void
setProgress
(
double
progress
)
{
_progress
=
progress
;
}
...
@@ -45,63 +47,58 @@ public:
...
@@ -45,63 +47,58 @@ public:
protected:
protected:
GeoContainer
_tile
;
GeoContainer
_tile
;
lo
ng
_id
;
QStri
ng
_id
;
double
_progress
;
double
_progress
;
};
};
typedef
GenericTile
<>
Tile
;
typedef
GenericTile
<>
Tile
;
template
<
class
TileType
>
template
<
class
TileType
>
bool
toJson
(
const
TileType
&
tile
,
rapidjson
::
Value
&
value
,
bool
toJson
(
const
TileType
&
tile
,
QJsonObject
&
value
)
{
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
rapidjson
;
value
[
idKey
]
=
tile
.
id
();
value
.
AddMember
(
Value
(
idKey
,
allocator
),
Value
(
tile
.
id
()),
allocator
);
value
[
progressKey
]
=
tile
.
progress
();
value
.
AddMember
(
Value
(
progressKey
,
allocator
),
Value
(
tile
.
progress
()),
allocator
);
using
namespace
messages
::
geographic_msgs
;
using
namespace
messages
::
geographic_msgs
;
rapidjson
::
Value
geoPoints
(
rapidjson
::
kArrayType
)
;
QJsonArray
geoPoints
;
for
(
unsigned
long
i
=
0
;
i
<
std
::
uint64_t
(
tile
.
tile
().
size
());
++
i
)
{
for
(
unsigned
long
i
=
0
;
i
<
std
::
uint64_t
(
tile
.
tile
().
size
());
++
i
)
{
rapidjson
::
Value
geoPoint
(
rapidjson
::
kObjectType
)
;
QJsonObject
geoPoint
;
if
(
!
geo_point
::
toJson
(
tile
.
tile
()[
i
],
geoPoint
,
allocator
))
{
if
(
!
geo_point
::
toJson
(
tile
.
tile
()[
i
],
geoPoint
))
{
return
false
;
return
false
;
}
}
geoPoints
.
PushBack
(
geoPoint
,
allocator
);
geoPoints
.
append
(
std
::
move
(
geoPoint
)
);
}
}
rapidjson
::
Value
key
(
tileKey
,
allocator
);
value
[
tileKey
]
=
std
::
move
(
geoPoints
);
value
.
AddMember
(
key
,
geoPoints
,
allocator
);
return
true
;
return
true
;
}
}
template
<
class
TileType
>
template
<
class
TileType
>
bool
fromJson
(
const
QJsonObject
&
value
,
TileType
&
p
)
{
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
TileType
&
p
)
{
if
(
!
value
.
contains
(
progressKey
)
||
!
value
[
progressKey
].
isDouble
())
{
if
(
!
value
.
HasMember
(
progressKey
)
||
!
value
[
progressKey
].
IsDouble
())
{
return
false
;
return
false
;
}
}
if
(
!
value
.
HasMember
(
idKey
)
||
!
value
[
idKey
].
IsInt
())
{
if
(
!
value
.
contains
(
idKey
)
||
!
value
[
idKey
].
isString
())
{
return
false
;
return
false
;
}
}
if
(
!
value
.
HasMember
(
tileKey
)
||
!
value
[
tileKey
].
I
sArray
())
{
if
(
!
value
.
contains
(
tileKey
)
||
!
value
[
tileKey
].
i
sArray
())
{
return
false
;
return
false
;
}
}
p
.
setId
(
value
[
idKey
].
GetInt
());
p
.
setId
(
value
[
idKey
].
toString
());
p
.
setProgress
(
value
[
progressKey
].
Get
Double
());
p
.
setProgress
(
value
[
progressKey
].
to
Double
());
using
namespace
geographic_msgs
;
using
namespace
geographic_msgs
;
const
auto
&
jsonArray
=
value
[
tileKey
].
Get
Array
();
const
auto
jsonArray
=
value
[
tileKey
].
to
Array
();
p
.
tile
().
clear
();
p
.
tile
().
clear
();
p
.
tile
().
reserve
(
jsonArray
.
S
ize
());
p
.
tile
().
reserve
(
jsonArray
.
s
ize
());
typedef
decltype
(
p
.
tile
()[
0
])
PointTypeCVR
;
typedef
decltype
(
p
.
tile
()[
0
])
PointTypeCVR
;
typedef
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
PointTypeCVR
>>
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
PointTypeCVR
>>
PointType
;
PointType
;
for
(
long
i
=
0
;
i
<
jsonArray
.
S
ize
();
++
i
)
{
for
(
long
i
=
0
;
i
<
jsonArray
.
s
ize
();
++
i
)
{
PointType
pt
;
PointType
pt
;
if
(
!
geo_point
::
fromJson
(
jsonArray
[
i
],
pt
))
{
if
(
!
geo_point
::
fromJson
(
jsonArray
[
i
],
pt
))
{
return
false
;
return
false
;
...
...
src/comm/ros_bridge/include/messages/std_msgs/header.cpp
deleted
100644 → 0
View file @
9e620065
#include "header.h"
std
::
uint32_t
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
_defaultSeq
=
0
;
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Header
()
:
_seq
(
_defaultSeq
++
),
_stamp
(
Time
()),
_frameId
(
""
)
{}
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Header
(
uint32_t
seq
,
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
stamp
,
const
std
::
string
&
frame_id
)
:
_seq
(
seq
)
,
_stamp
(
stamp
)
,
_frameId
(
frame_id
)
{}
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Header
(
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
&
header
)
:
_seq
(
header
.
seq
())
,
_stamp
(
header
.
stamp
())
,
_frameId
(
header
.
frameId
())
{}
uint32_t
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
seq
()
const
{
return
_seq
;
}
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
stamp
()
const
{
return
_stamp
;
}
const
std
::
string
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
frameId
()
const
{
return
_frameId
;
}
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
stamp
()
{
return
_stamp
;
}
std
::
string
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
frameId
()
{
return
_frameId
;
}
void
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
setSeq
(
uint32_t
seq
)
{
_seq
=
seq
;
}
void
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
setStamp
(
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
stamp
)
{
_stamp
=
stamp
;
}
void
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
setFrameId
(
const
std
::
string
&
frameId
)
{
_frameId
=
frameId
;
}
std
::
string
ros_bridge
::
messages
::
std_msgs
::
header
::
messageType
(){
return
"std_msgs/Header"
;
}
src/comm/ros_bridge/include/messages/std_msgs/header.h
deleted
100644 → 0
View file @
9e620065
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/std_msgs/time.h"
#include <string>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for std_msgs generation.
namespace
std_msgs
{
//! @brief Namespace containing classes and methodes for std_msgs/Header message generation.
namespace
header
{
std
::
string
messageType
();
//! @brief C++ representation of std_msgs/Header
class
Header
{
public:
using
Time
=
std_msgs
::
time
::
Time
;
Header
();
Header
(
uint32_t
seq
,
const
Time
&
stamp
,
const
std
::
string
&
frame_id
);
Header
(
const
Header
&
header
);
uint32_t
seq
()
const
;
const
Time
&
stamp
()
const
;
const
std
::
string
&
frameId
()
const
;
Time
&
stamp
();
std
::
string
&
frameId
();
void
setSeq
(
uint32_t
seq
);
void
setStamp
(
const
Time
&
stamp
);
void
setFrameId
(
const
std
::
string
&
frameId
);
private:
static
uint32_t
_defaultSeq
;
uint32_t
_seq
;
Time
_stamp
;
std
::
string
_frameId
;
};
template
<
class
HeaderType
>
bool
toJson
(
const
HeaderType
&
header
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
messages
::
std_msgs
;
value
.
AddMember
(
"seq"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
header
.
seq
())),
allocator
);
rapidjson
::
Value
stamp
(
rapidjson
::
kObjectType
);
if
(
!
time
::
toJson
(
header
.
stamp
(),
stamp
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"stamp"
,
stamp
,
allocator
);
value
.
AddMember
(
"frame_id"
,
rapidjson
::
Value
().
SetString
(
header
.
frameId
().
data
(),
header
.
frameId
().
length
(),
allocator
),
allocator
);
return
true
;
}
template
<
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
HeaderType
&
header
)
{
using
namespace
messages
::
std_msgs
;
if
(
!
value
.
HasMember
(
"seq"
)
||
!
value
[
"seq"
].
IsUint
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"stamp"
)){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"frame_id"
)
||
!
value
[
"frame_id"
].
IsString
()){
assert
(
false
);
return
false
;
}
header
.
setSeq
(
value
[
"seq"
].
GetUint
());
decltype
(
header
.
stamp
())
time
;
if
(
!
time
::
fromJson
(
value
[
"stamp"
],
time
)){
assert
(
false
);
return
false
;
}
header
.
setStamp
(
time
);
header
.
setFrameId
(
value
[
"frame_id"
].
GetString
());
return
true
;
}
}
// namespace time
}
// namespace std_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/std_msgs/time.cpp
deleted
100644 → 0
View file @
9e620065
#include "time.h"
std
::
string
ros_bridge
::
messages
::
std_msgs
::
time
::
messageType
(){
return
"std_msgs/Time"
;
}
src/comm/ros_bridge/include/messages/std_msgs/time.h
deleted
100644 → 0
View file @
9e620065
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for std_msgs generation.
namespace
std_msgs
{
//! @brief Namespace containing classes and methodes for std_msgs/Time message generation.
namespace
time
{
std
::
string
messageType
();
//! @brief C++ representation of std_msgs/Time
class
Time
{
public:
Time
()
:
_secs
(
0
),
_nsecs
(
0
)
{}
Time
(
uint32_t
secs
,
uint32_t
nsecs
)
:
_secs
(
secs
),
_nsecs
(
nsecs
)
{}
Time
(
const
Time
&
time
)
:
_secs
(
time
.
secs
()),
_nsecs
(
time
.
nSecs
())
{}
uint32_t
secs
()
const
{
return
_secs
;}
uint32_t
nSecs
()
const
{
return
_nsecs
;}
void
setSecs
(
uint32_t
secs
)
{
_secs
=
secs
;}
void
setNSecs
(
uint32_t
nsecs
)
{
_nsecs
=
nsecs
;}
private:
uint32_t
_secs
;
uint32_t
_nsecs
;
};
template
<
class
TimeType
>
bool
toJson
(
const
TimeType
&
time
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"secs"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
time
.
secs
())),
allocator
);
value
.
AddMember
(
"nsecs"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
time
.
nSecs
())),
allocator
);
return
true
;
}
template
<
class
TimeType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
TimeType
&
time
)
{
if
(
!
value
.
HasMember
(
"secs"
)
||
!
value
[
"secs"
].
IsUint
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"nsecs"
)
||
!
value
[
"nsecs"
].
IsUint
()){
assert
(
false
);
return
false
;
}
time
.
setSecs
(
value
[
"secs"
].
GetUint
());
time
.
setNSecs
(
value
[
"nsecs"
].
GetUint
());
return
true
;
}
}
// namespace time
}
// namespace std_msgs
}
// namespace messages
}
// namespace ros_bridge
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