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
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 += \
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress_array
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
tile
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
time
.
h
\
src
/
comm
/
utilities
.
h
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
...
...
@@ -560,8 +558,6 @@ SOURCES += \
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
/
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
\
contains
(
DEFINES
,
QGC_ENABLE_PAIRING
)
{
...
...
src/MeasurementComplexItem/IDArray.h
View file @
b098b99a
...
...
@@ -3,6 +3,6 @@
#include <QVector>
typedef
QVector
<
std
::
int64_t
>
IDArray
;
typedef
QVector
<
QString
>
IDArray
;
#endif // IDARRAY_H
src/MeasurementComplexItem/MeasurementComplexItem.h
View file @
b098b99a
...
...
@@ -10,7 +10,7 @@
#include "SettingsFact.h"
#include "AreaData.h"
#include "ProgressArray.h"
#include "
geometry/
ProgressArray.h"
class
RoutingThread
;
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 @@
#define MAX_TILES 1000
#endif
QString
randomId
();
using
namespace
geometry
;
namespace
trans
=
bg
::
strategy
::
transform
;
...
...
@@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
// find unique id
if
(
it
!=
_indexMap
.
end
())
{
auto
newId
=
tile
->
id
()
+
1
;
auto
newId
=
MeasurementTile
::
randomId
()
;
constexpr
long
counterMax
=
1e6
;
unsigned
long
counter
=
0
;
for
(;
counter
<=
counterMax
;
++
counter
)
{
...
...
@@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
if
(
it
==
_indexMap
.
end
())
{
break
;
}
else
{
++
newId
;
newId
=
MeasurementTile
::
randomId
()
;
}
}
...
...
@@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() {
// Convert to geo system.
for
(
const
auto
&
t
:
tilesENU
)
{
auto
geoTile
=
new
MeasurementTile
(
pData
.
get
());
std
::
size_t
hashValue
=
0
;
std
::
hash
<
QGeoCoordinate
>
hashFun
;
for
(
const
auto
&
v
:
t
.
outer
())
{
QGeoCoordinate
geoVertex
;
fromENU
(
origin
,
v
,
geoVertex
);
geoTile
->
push_back
(
geoVertex
);
hashValue
^=
hashFun
(
geoVertex
);
}
geoTile
->
setId
(
std
::
int64_t
(
hashValue
));
pData
->
append
(
geoTile
);
}
}
...
...
@@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() {
// find unique id
if
(
it
!=
_indexMap
.
end
())
{
auto
newId
=
tile
->
id
()
+
1
;
auto
newId
=
MeasurementTile
::
randomId
()
;
constexpr
long
counterMax
=
1e6
;
unsigned
long
counter
=
0
;
for
(;
counter
<=
counterMax
;
++
counter
)
{
...
...
@@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() {
if
(
it
==
_indexMap
.
end
())
{
break
;
}
else
{
++
newId
;
newId
=
MeasurementTile
::
randomId
()
;
}
}
...
...
@@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
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:
// Tile stuff.
TilePtr
_tiles
;
std
::
map
<
std
::
int64_t
/*id*/
,
int
>
_indexMap
;
std
::
map
<
QString
/*id*/
,
int
>
_indexMap
;
QTimer
_timer
;
STATE
_state
;
QFutureWatcher
<
TilePtr
>
_watcher
;
...
...
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp
View file @
b098b99a
...
...
@@ -6,7 +6,7 @@ const char *MeasurementTile::settingsGroup = "MeasurementTile";
const
char
*
MeasurementTile
::
nameString
=
"MeasurementTile"
;
MeasurementTile
::
MeasurementTile
(
QObject
*
parent
)
:
GeoArea
(
parent
),
_progress
(
0
),
_id
(
0
)
{
:
GeoArea
(
parent
),
_progress
(
0
),
_id
(
MeasurementTile
::
randomId
()
)
{
init
();
}
...
...
@@ -38,9 +38,9 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
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
)
{
_id
=
id
;
emit
idChanged
();
...
...
@@ -49,6 +49,19 @@ void MeasurementTile::setId(const int64_t &id) {
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
;
}
void
MeasurementTile
::
setProgress
(
double
progress
)
{
...
...
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h
View file @
b098b99a
...
...
@@ -24,11 +24,13 @@ public:
double
progress
()
const
;
void
setProgress
(
double
progress
);
int64_t
id
()
const
;
void
setId
(
const
int64_t
&
id
);
QString
id
()
const
;
void
setId
(
const
QString
&
id
);
QList
<
QGeoCoordinate
>
tile
()
const
;
static
QString
randomId
(
int
length
=
10
);
// Static Variables
static
const
char
*
settingsGroup
;
static
const
char
*
nameString
;
...
...
@@ -40,5 +42,5 @@ signals:
private:
void
init
();
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() {
return
this
->
_taskQueue
.
size
()
+
(
_running
?
1
:
0
);
}
bool
TaskDispatcher
::
idle
()
{
return
this
->
pendingTasks
()
==
0
;
}
void
TaskDispatcher
::
run
()
{
while
(
true
)
{
ULock
lk1
(
this
->
_mutex
);
...
...
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h
View file @
b098b99a
...
...
@@ -53,6 +53,7 @@ public:
bool
isRunning
();
std
::
size_t
pendingTasks
();
bool
idle
();
protected:
void
run
();
...
...
src/MeasurementComplexItem/rosbridge/rosbridge.cpp
View file @
b098b99a
...
...
@@ -16,6 +16,8 @@ static const char *rosTopics = "/rosapi/topics";
static
constexpr
auto
pollIntervall
=
std
::
chrono
::
milliseconds
(
200
);
Rosbridge
::
STATE
translate
(
RosbridgeImpl
::
STATE
s
);
Rosbridge
::
Rosbridge
(
const
QUrl
url
,
QObject
*
parent
)
:
QObject
(
parent
),
_url
(
url
),
_running
(
false
)
{
static
std
::
once_flag
flag
;
...
...
@@ -34,6 +36,9 @@ void Rosbridge::start() {
_impl
=
new
RosbridgeImpl
(
_url
);
_impl
->
moveToThread
(
_t
);
connect
(
_impl
,
&
RosbridgeImpl
::
stateChanged
,
this
,
&
Rosbridge
::
_onImplStateChanged
);
connect
(
this
,
&
Rosbridge
::
_start
,
_impl
,
&
RosbridgeImpl
::
start
);
connect
(
this
,
&
Rosbridge
::
_stop
,
_impl
,
&
RosbridgeImpl
::
stop
);
...
...
@@ -77,20 +82,7 @@ void Rosbridge::stop() {
Rosbridge
::
STATE
Rosbridge
::
state
()
{
if
(
_running
)
{
switch
(
_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
;
return
translate
(
_impl
->
state
());
}
else
{
return
STATE
::
STOPPED
;
}
...
...
@@ -251,3 +243,30 @@ void Rosbridge::waitForService(const QString &service) {
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:
void
_unadvertiseAllServices
();
private:
void
_onImplStateChanged
();
std
::
atomic
<
STATE
>
_state
;
RosbridgeImpl
*
_impl
;
QThread
*
_t
;
QUrl
_url
;
...
...
src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp
View file @
b098b99a
...
...
@@ -114,7 +114,6 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
auto
it
=
_advertisedTopics
.
find
(
topic
);
if
(
Q_LIKELY
(
it
!=
_advertisedTopics
.
end
()))
{
_advertisedTopics
.
erase
(
it
);
QJsonObject
o
;
o
[
opKey
]
=
unadvertiseOpKey
;
...
...
@@ -123,6 +122,8 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
QString
payload
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
_webSocket
.
sendTextMessage
(
payload
);
_advertisedTopics
.
erase
(
it
);
}
else
{
qDebug
()
<<
"Topic "
<<
topic
<<
" not advertised."
;
}
...
...
@@ -170,7 +171,6 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
auto
it
=
_subscribedTopics
.
find
(
topic
);
if
(
Q_LIKELY
(
it
!=
_subscribedTopics
.
end
()))
{
_subscribedTopics
.
erase
(
it
);
QJsonObject
o
;
o
[
opKey
]
=
unsubscribeOpKey
;
...
...
@@ -179,6 +179,8 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
QString
payload
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
_webSocket
.
sendTextMessage
(
payload
);
_subscribedTopics
.
erase
(
it
);
}
else
{
qDebug
()
<<
"unsubscribeTopic: topic "
<<
topic
<<
" already subscribed!"
;
}
...
...
@@ -293,7 +295,6 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
if
(
_state
==
STATE
::
CONNECTED
||
_state
==
STATE
::
STOPPING
)
{
auto
it
=
_advertisedServices
.
find
(
service
);
if
(
it
!=
_advertisedServices
.
end
())
{
it
=
_advertisedServices
.
erase
(
it
);
QJsonObject
o
;
o
[
opKey
]
=
unadvertiseServiceOpKey
;
...
...
@@ -302,6 +303,8 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
QString
payload
=
QJsonDocument
(
o
).
toJson
(
QJsonDocument
::
JsonFormat
::
Compact
);
_webSocket
.
sendTextMessage
(
payload
);
it
=
_advertisedServices
.
erase
(
it
);
}
else
{
qDebug
()
<<
"unadvertiseService: Service "
<<
service
<<
" not advertised."
;
...
...
@@ -369,6 +372,7 @@ void RosbridgeImpl::_doAction() {
}
void
RosbridgeImpl
::
_onTextMessageReceived
(
const
QString
&
message
)
{
qDebug
()
<<
"_onTextMessageReceived: "
<<
message
;
QJsonParseError
e
;
auto
d
=
QJsonDocument
::
fromJson
(
message
.
toUtf8
(),
&
e
);
if
(
!
d
.
isNull
())
{
...
...
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
View file @
b098b99a
...
...
@@ -3,147 +3,140 @@
#include <string>
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QJsonObject>
#include <QJsonValue>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
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
{
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message
//! generation.
namespace
geo_point
{
std
::
string
messageType
();
namespace
{
const
char
*
lonKey
=
"longitude"
;
const
char
*
latKey
=
"latitude"
;
const
char
*
altKey
=
"altitude"
;
}
// namespace
//! @brief C++ representation of geographic_msgs/GeoPoint.
template
<
class
FloatType
=
_Float64
,
class
OStream
=
std
::
ostream
>
class
GenericGeoPoint
{
template
<
class
FloatType
=
_Float64
,
class
OStream
=
std
::
ostream
>
class
GenericGeoPoint
{
public:
GenericGeoPoint
()
:
_latitude
(
0
)
,
_longitude
(
0
)
,
_altitude
(
0
)
{}
GenericGeoPoint
(
const
GenericGeoPoint
&
other
)
:
_latitude
(
other
.
latitude
())
,
_longitude
(
other
.
longitude
())
,
_altitude
(
other
.
altitude
())
{}
GenericGeoPoint
(
FloatType
latitude
,
FloatType
longitude
,
FloatType
altitude
)
:
_latitude
(
latitude
)
,
_longitude
(
longitude
)
,
_altitude
(
altitude
)
{}
FloatType
latitude
()
const
{
return
_latitude
;}
FloatType
longitude
()
const
{
return
_longitude
;}
FloatType
altitude
()
const
{
return
_altitude
;}
void
setLatitude
(
FloatType
latitude
)
{
_latitude
=
latitude
;}
void
setLongitude
(
FloatType
longitude
)
{
_longitude
=
longitude
;}
void
setAltitude
(
FloatType
altitude
)
{
_altitude
=
altitude
;}
bool
operator
==
(
const
GenericGeoPoint
&
p1
)
{
return
(
p1
.
_latitude
==
this
->
_latitude
&&
p1
.
_longitude
==
this
->
_longitude
&&
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
;
}
GenericGeoPoint
()
:
_latitude
(
0
),
_longitude
(
0
),
_altitude
(
0
)
{}
GenericGeoPoint
(
const
GenericGeoPoint
&
other
)
:
_latitude
(
other
.
latitude
()),
_longitude
(
other
.
longitude
()),
_altitude
(
other
.
altitude
())
{}
GenericGeoPoint
(
FloatType
latitude
,
FloatType
longitude
,
FloatType
altitude
)
:
_latitude
(
latitude
),
_longitude
(
longitude
),
_altitude
(
altitude
)
{}
FloatType
latitude
()
const
{
return
_latitude
;
}
FloatType
longitude
()
const
{
return
_longitude
;
}
FloatType
altitude
()
const
{
return
_altitude
;
}
void
setLatitude
(
FloatType
latitude
)
{
_latitude
=
latitude
;
}
void
setLongitude
(
FloatType
longitude
)
{
_longitude
=
longitude
;
}
void
setAltitude
(
FloatType
altitude
)
{
_altitude
=
altitude
;
}
bool
operator
==
(
const
GenericGeoPoint
&
p1
)
{
return
(
p1
.
_latitude
==
this
->
_latitude
&&
p1
.
_longitude
==
this
->
_longitude
&&
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:
FloatType
_latitude
;
FloatType
_longitude
;
FloatType
_altitude
;
FloatType
_latitude
;
FloatType
_longitude
;
FloatType
_altitude
;
};
typedef
GenericGeoPoint
<>
GeoPoint
;
namespace
detail
{
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
return
p
.
altitude
();
}
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
}
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
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
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
return
p
.
altitude
();
}
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
}
template
<
class
T
>
void
setAltitude
(
const
QJsonValue
&
o
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
p
.
setAltitude
(
o
.
toDouble
());
}
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
value
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
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
;
void
setAltitude
(
const
QJsonValue
&
o
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
(
void
)
o
;
(
void
)
p
;
}
}
// 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
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
if
(
!
value
.
HasMember
(
"latitude"
)
||
!
value
[
"latitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"longitude"
)
||
!
value
[
"longitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"altitude"
)
||
!
value
[
"altitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setLatitude
(
value
[
"latitude"
].
GetFloat
());
p
.
setLongitude
(
value
[
"longitude"
].
GetFloat
());
typedef
typename
traits
::
Select
<
traits
::
HasMemberSetAltitude
<
PointType
>::
value
,
traits
::
Has3Components
,
traits
::
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
detail
::
setAltitude
(
value
[
"altitude"
],
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude() discard doc["altitude"];
return
true
;
bool
fromJson
(
const
QJsonObject
&
value
,
PointType
&
p
)
{
if
(
!
value
.
contains
(
"latitude"
)
||
!
value
[
"latitude"
].
isDouble
())
{
return
false
;
}
if
(
!
value
.
contains
(
"longitude"
)
||
!
value
[
"longitude"
].
isDouble
())
{
return
false
;
}
if
(
!
value
.
contains
(
"altitude"
)
||
!
value
[
"altitude"
].
isDouble
())
{
return
false
;
}
p
.
setLatitude
(
value
[
"latitude"
].
toDouble
());
p
.
setLongitude
(
value
[
"longitude"
].
toDouble
());
typedef
typename
traits
::
Select
<
traits
::
HasMemberSetAltitude
<
PointType
>::
value
,
traits
::
Has3Components
,
traits
::
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
detail
::
setAltitude
(
value
[
"altitude"
],
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude() discard
// doc["altitude"];
return
true
;
}
// namespace detail
}
// namespace geopoint
}
// namespace geometry_msgs
}
// namespace geo_point
}
// namespace geographic_msgs
}
// namespace messages
}
// 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
)
{