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
Expand all
Show 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
This diff is collapsed.
Click to expand it.
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,59 +3,59 @@
...
@@ -3,59 +3,59 @@
#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
)
,
_longitude
(
0
)
,
_altitude
(
0
)
{}
GenericGeoPoint
(
const
GenericGeoPoint
&
other
)
GenericGeoPoint
(
const
GenericGeoPoint
&
other
)
:
_latitude
(
other
.
latitude
())
:
_latitude
(
other
.
latitude
()),
_longitude
(
other
.
longitude
()),
,
_longitude
(
other
.
longitude
())
_altitude
(
other
.
altitude
())
{}
,
_altitude
(
other
.
altitude
())
{}
GenericGeoPoint
(
FloatType
latitude
,
FloatType
longitude
,
FloatType
altitude
)
GenericGeoPoint
(
FloatType
latitude
,
FloatType
longitude
,
FloatType
altitude
)
:
_latitude
(
latitude
)
:
_latitude
(
latitude
),
_longitude
(
longitude
),
_altitude
(
altitude
)
{}
,
_longitude
(
longitude
)
,
_altitude
(
altitude
)
{}
FloatType
latitude
()
const
{
return
_latitude
;
}
FloatType
latitude
()
const
{
return
_latitude
;
}
FloatType
longitude
()
const
{
return
_longitude
;
}
FloatType
longitude
()
const
{
return
_longitude
;
}
FloatType
altitude
()
const
{
return
_altitude
;
}
FloatType
altitude
()
const
{
return
_altitude
;
}
void
setLatitude
(
FloatType
latitude
)
{
_latitude
=
latitude
;
}
void
setLatitude
(
FloatType
latitude
)
{
_latitude
=
latitude
;
}
void
setLongitude
(
FloatType
longitude
)
{
_longitude
=
longitude
;
}
void
setLongitude
(
FloatType
longitude
)
{
_longitude
=
longitude
;
}
void
setAltitude
(
FloatType
altitude
)
{
_altitude
=
altitude
;
}
void
setAltitude
(
FloatType
altitude
)
{
_altitude
=
altitude
;
}
bool
operator
==
(
const
GenericGeoPoint
&
p1
)
{
bool
operator
==
(
const
GenericGeoPoint
&
p1
)
{
return
(
p1
.
_latitude
==
this
->
_latitude
return
(
p1
.
_latitude
==
this
->
_latitude
&&
&&
p1
.
_longitude
==
this
->
_longitude
p1
.
_longitude
==
this
->
_longitude
&&
&&
p1
.
_altitude
==
this
->
_altitude
);
p1
.
_altitude
==
this
->
_altitude
);
}
}
bool
operator
!=
(
const
GenericGeoPoint
&
p1
)
{
bool
operator
!=
(
const
GenericGeoPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
return
!
this
->
operator
==
(
p1
);
}
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericGeoPoint
&
p
)
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericGeoPoint
&
p
)
{
{
os
<<
"lat: "
<<
p
.
_latitude
<<
" lon: "
<<
p
.
_longitude
os
<<
"lat: "
<<
p
.
_latitude
<<
" lon: "
<<
p
.
_longitude
<<
" alt: "
<<
p
.
_altitude
;
<<
" alt: "
<<
p
.
_altitude
;
return
os
;
return
os
;
}
}
...
@@ -68,82 +68,75 @@ typedef GenericGeoPoint<> GeoPoint;
...
@@ -68,82 +68,75 @@ 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
;
(
void
)
p
;
return
0
.
0
;
return
0
.
0
;
}
}
template
<
class
T
>
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
void
setAltitude
(
const
QJsonValue
&
o
,
T
&
p
,
{
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
p
.
setAltitude
(
doc
.
GetFloat
());
p
.
setAltitude
(
o
.
toDouble
());
}
}
template
<
class
T
>
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
void
setAltitude
(
const
QJsonValue
&
o
,
T
&
p
,
{
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
o
;
(
void
)
p
;
(
void
)
p
;
}
}
}
// namespace detail
}
// namespace detail
template
<
class
T
>
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
QJsonObject
&
value
)
{
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
value
[
latKey
]
=
p
.
latitude
();
{
value
[
lonKey
]
=
p
.
longitude
();
value
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
value
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
typedef
typename
traits
::
Select
<
typedef
typename
traits
::
Select
<
traits
::
HasMemberAltitude
<
T
>::
value
,
traits
::
HasMemberAltitude
<
T
>::
value
,
traits
::
Has3Components
,
traits
::
Has3Components
,
traits
::
Has2Components
>::
Result
traits
::
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
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;
auto
altitude
=
detail
::
getAltitude
(
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude()
// replace it by 0.0;
value
.
AddMember
(
"altitude"
,
rapidjson