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
1de3b8e6
Commit
1de3b8e6
authored
Jul 08, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
code not compilable.
parent
3409524b
Changes
25
Hide whitespace changes
Inline
Side-by-side
Showing
25 changed files
with
867 additions
and
224 deletions
+867
-224
qgroundcontrol.pro
qgroundcontrol.pro
+5
-8
.directory
src/Wima/.directory
+4
-0
GeoPoint3D.h
src/Wima/GeoPoint3D.h
+8
-7
Polygon2D.h
src/Wima/Polygon2D.h
+5
-4
PolygonArray.cc
src/Wima/PolygonArray.cc
+0
-3
PolygonArray.h
src/Wima/PolygonArray.h
+5
-7
QtROSJsonFactory.h
src/Wima/QtROSJsonFactory.h
+3
-2
QtROSTypeFactory.h
src/Wima/QtROSTypeFactory.h
+5
-0
ROSCommunicator.cpp
src/Wima/ROSCommunicator.cpp
+0
-6
ROSCommunicator.h
src/Wima/ROSCommunicator.h
+0
-24
ROSJsonFactory.cpp
src/Wima/ROSJsonFactory.cpp
+0
-2
ROSMessageGroups.h
src/Wima/ROSMessageGroups.h
+0
-45
SnakeTiles.h
src/Wima/SnakeTiles.h
+4
-3
SnakeTilesLocal.h
src/Wima/SnakeTilesLocal.h
+7
-3
WimaController.cc
src/Wima/WimaController.cc
+23
-5
WimaController.h
src/Wima/WimaController.h
+5
-9
WimaPolygonArray.h
src/Wima/WimaPolygonArray.h
+6
-4
JsonFactory.h
src/comm/ros_bridge/include/JsonFactory.h
+52
-51
JsonMethodes.h
src/comm/ros_bridge/include/JsonMethodes.h
+372
-39
MessageBaseClass.h
src/comm/ros_bridge/include/MessageBaseClass.h
+29
-0
MessageGroups.h
src/comm/ros_bridge/include/MessageGroups.h
+114
-0
MessageTraits.h
src/comm/ros_bridge/include/MessageTraits.h
+86
-2
ROSCommunicator.h
src/comm/ros_bridge/include/ROSCommunicator.h
+17
-0
TypeFactory.h
src/comm/ros_bridge/include/TypeFactory.h
+116
-0
ROSCommunicator.cpp
src/comm/ros_bridge/src/ROSCommunicator.cpp
+1
-0
No files found.
qgroundcontrol.pro
View file @
1de3b8e6
...
...
@@ -433,10 +433,7 @@ HEADERS += \
src
/
Wima
/
Polygon2D
.
h
\
src
/
Wima
/
PolygonArray
.
h
\
src
/
Wima
/
QtROSJsonFactory
.
h
\
src
/
Wima
/
ROSCommunicator
.
h
\
src
/
Wima
/
ROSJsonFactory
.
h
\
src
/
Wima
/
ROSMessageGroups
.
h
\
src
/
Wima
/
ROSMessageType
.
h
\
src
/
Wima
/
QtROSTypeFactory
.
h
\
src
/
Wima
/
SnakeTiles
.
h
\
src
/
Wima
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
WimaControllerDetail
.
h
\
...
...
@@ -475,8 +472,9 @@ HEADERS += \
src
/
Wima
/
testplanimetrycalculus
.
h
\
src
/
Settings
/
WimaSettings
.
h
\
src
/
QmlControls
/
QmlObjectVectorModel
.
h
\
src
/
comm
/
ros_bridge
/
include
/
ROSMessageTraits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
.
h
\
src
/
comm
/
ros_bridge
/
include
/
JsonMethodes
.
h
\
src
/
comm
/
ros_bridge
/
include
/
MessageTraits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TypeFactory
.
h
\
src
/
comm
/
utilities
.
h
SOURCES
+=
\
src
/
Snake
/
clipper
/
clipper
.
cpp
\
...
...
@@ -484,8 +482,7 @@ SOURCES += \
src
/
Snake
/
snake_geometry
.
cpp
\
src
/
Wima
/
GeoPoint3D
.
cpp
\
src
/
Wima
/
PolygonArray
.
cc
\
src
/
Wima
/
ROSCommunicator
.
cpp
\
src
/
Wima
/
ROSJsonFactory
.
cpp
\
src
/
comm
/
ros_bridge
/
src
/
ROSCommunicator
.
cpp
\
src
/
Wima
/
WimaControllerDetail
.
cc
\
src
/
Wima
/
snaketile
.
cpp
\
src
/
api
/
QGCCorePlugin
.
cc
\
...
...
src/Wima/.directory
0 → 100644
View file @
1de3b8e6
[Dolphin]
Timestamp=2020,7,8,11,13,28
Version=4
ViewMode=1
src/Wima/GeoPoint3D.h
View file @
1de3b8e6
#pragma once
#include "ros_bridge/include/
messag
es.h"
#include "
ROSMessageType
.h"
#include "
ROS
MessageGroups.h"
#include "ros_bridge/include/
JsonMethod
es.h"
#include "
ros_bridge/include/MessageBaseClass
.h"
#include "
ros_bridge/include/
MessageGroups.h"
#include <QObject>
typedef
ROSMessageType
<
QString
>
ROSMsg
;
typedef
ros_bridge
::
GeoPoint
::
GeoPoint
ROSGeoPoint
;
typedef
ROSBridge
::
MessageBaseClass
<
QString
>
ROSMsg
;
typedef
ROSBridge
::
JsonMethodes
::
GeoPoint
::
GeoPoint
ROSGeoPoint
;
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
class
GeoPoint3D
:
public
QObject
,
public
ROSMsg
,
public
ROSGeoPoint
{
Q_OBJECT
public:
typedef
GeoPointGroup
Group
;
typedef
MsgGroups
::
GeoPointGroup
Group
;
explicit
GeoPoint3D
(
QObject
*
parent
=
nullptr
)
:
QObject
(
parent
),
ROSMsg
(),
ROSGeoPoint
()
{}
...
...
@@ -25,7 +26,7 @@ public:
QObject
*
parent
=
nullptr
)
:
QObject
(
parent
),
ROSMsg
(),
ROSGeoPoint
(
p
.
latitude
(),
p
.
longitude
(),
p
.
altitude
())
{}
explicit
GeoPoint3D
(
const
class
ros_bridge
::
GeoPoint
::
GeoPoint
&
p
,
explicit
GeoPoint3D
(
const
ROS
GeoPoint
&
p
,
QObject
*
parent
=
nullptr
)
:
QObject
(
parent
),
ROSMsg
(),
ROSGeoPoint
(
p
.
latitude
(),
p
.
longitude
(),
p
.
altitude
())
{}
...
...
src/Wima/Polygon2D.h
View file @
1de3b8e6
...
...
@@ -3,14 +3,15 @@
#include <QPolygonF>
#include <QPointF>
#include "
ROS
MessageGroups.h"
#include "
ROSMessageType
.h"
#include "
ros_bridge/include/
MessageGroups.h"
#include "
ros_bridge/include/MessageBaseClass
.h"
typedef
ROSMessageType
<
QString
>
ROSMsg
;
typedef
ROSBridge
::
MessageBaseClass
<
QString
>
ROSMsg
;
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
class
Polygon2D
:
public
QPolygonF
,
public
ROSMsg
{
public:
typedef
PolygonGroup
Group
;
typedef
MsgGroups
::
PolygonGroup
Group
;
Polygon2D
(){}
Polygon2D
(
const
Polygon2D
&
other
)
:
QPolygonF
(
other
)
,
ROSMsg
(){}
...
...
src/Wima/PolygonArray.cc
View file @
1de3b8e6
#include "PolygonArray.h"
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
>
const
char
*
PolygonArray
<
PolygonType
,
ContainerType
>::
typeName
=
"PolygonArray"
;
src/Wima/PolygonArray.h
View file @
1de3b8e6
...
...
@@ -2,16 +2,14 @@
#include <QObject>
#include "ROSMessageType.h"
#include "ros_bridge/include/MessageBaseClass.h"
typedef
ROSBridge
::
MessageBaseClass
<
QString
>
ROSMsgBase
;
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
>
class
PolygonArray
:
public
ROSM
essageType
<
QString
>
,
public
ContainerType
<
PolygonType
>
{
class
PolygonArray
:
public
ROSM
sgBase
,
public
ContainerType
<
PolygonType
>
{
public:
explicit
PolygonArray
()
:
ROSM
essageType
<
QString
>
(),
ContainerType
<
PolygonType
>
()
{}
explicit
PolygonArray
()
:
ROSM
sgBase
(),
ContainerType
<
PolygonType
>
()
{}
PolygonArray
(
const
PolygonArray
&
other
)
:
ContainerType
<
PolygonType
>
(
other
)
{}
QString
type
()
const
override
{
return
typeName
;}
static
const
char
*
typeName
;
QString
type
()
const
override
{
return
"PolygonArray"
;}
};
src/Wima/QtROSJsonFactory.h
View file @
1de3b8e6
#pragma once
#include "ROSJsonFactory.h"
#include "ros_bridge/include/JsonFactory.h"
#include <QString>
typedef
ROS
JsonFactory
<
>
QtROSJsonFactory
;
typedef
ROS
Bridge
::
JsonFactory
<
QString
>
QtROSJsonFactory
;
src/Wima/QtROSTypeFactory.h
0 → 100644
View file @
1de3b8e6
#pragma once
#include "ros_bridge/include/TypeFactory.h"
#include <QString>
typedef
ROSBridge
::
TypeFactory
<
QString
>
QtROSTypeFactory
;
src/Wima/ROSCommunicator.cpp
deleted
100644 → 0
View file @
3409524b
#include "ROSCommunicator.h"
void
ROSCommunicator
::
send
(
const
ROSMsg
*
msg
)
{
}
src/Wima/ROSCommunicator.h
deleted
100644 → 0
View file @
3409524b
#pragma once
#include "WimaAreaData.h"
#include "ROSMessageType.h"
typedef
ROSMessageType
<
QString
>
ROSMsg
;
class
ROSCommunicator
:
public
QObject
{
Q_OBJECT
public:
explicit
ROSCommunicator
(
QObject
*
parent
=
0
)
:
QObject
(
parent
)
{}
void
send
(
const
ROSMsg
*
msg
);
public
slots
:
virtual
void
receive
(
ROSMsg
*
msg
)
=
0
;
signals:
void
readAsynkPolledChanged
(
int
value
);
};
src/Wima/ROSJsonFactory.cpp
deleted
100644 → 0
View file @
3409524b
#include "ROSJsonFactory.h"
#include "PolygonArray.h"
src/Wima/ROSMessageGroups.h
deleted
100644 → 0
View file @
3409524b
#pragma once
namespace
detail
{
}
//!
//! \note Each calls belonging to a ROS message group must derive from \class ROSMessageType.
//!
//! \brief The EmptyGroup struct is used by the \class ROSMessageType base class.
//!
//! The EmptyGroup struct is used by the \class ROSMessageType base class. Passing a class using this
//! group will to the \class ROSJsonFactory will lead to a compile-time error.
struct
EmptyGroup
{};
//!
//! \brief The Point32Group struct is used the mark a class as a ROS Point32 message.
//!
//! The Point32Group struct is used the mark a class as a ROS Point32 message.
struct
Point32Group
{};
//!
//! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
//!
//! The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
struct
GeoPointGroup
{};
//!
//! \brief The PolygonGroup struct is used the mark a class as a ROS Polygon message.
//!
//! The PolygonGroup struct is used the mark a class as a ROS Polygon message.
struct
PolygonGroup
{};
//!
//! \brief The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
//!
//! The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
struct
PolygonStampedGroup
{};
//!
//! \brief The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
//!
//! The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
struct
PolygonArrayGroup
{};
src/Wima/SnakeTiles.h
View file @
1de3b8e6
#ifndef SNAKETILES_H
#define SNAKETILES_H
#pragma once
#include "snaketile.h"
#include "WimaPolygonArray.h"
#endif // SNAKETILES_H
typedef
WimaPolygonArray
<
SnakeTile
,
QVector
>
SnakeTiles
;
src/Wima/SnakeTilesLocal.h
View file @
1de3b8e6
#ifndef SNAKETILELOCAL_H
#define SNAKETILELOCAL_H
#pragma once
#endif // SNAKETILELOCAL_H
#include "ros_bridge/include/MessageGroups.h"
#include "Polygon2D.h"
#include "WimaPolygonArray.h"
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
typedef
WimaPolygonArray
<
Polygon2D
,
QVector
,
MsgGroups
::
PolygonArrayGroup
>
SnakeTilesLocal
;
src/Wima/WimaController.cc
View file @
1de3b8e6
#include "WimaController.h"
#include "utilities.h"
#include "ros_bridge/include/
messag
es.h"
#include "ros_bridge/include/
JsonMethod
es.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "QtROSJsonFactory.h"
#include "QtROSTypeFactory.h"
#include "time.h"
#include "assert.h"
...
...
@@ -613,13 +614,14 @@ bool WimaController::_fetchContainerData()
_snakeTilesLocal
.
polygons
().
append
(
Tile
);
}
QtROSJsonFactory
f
actory
;
QScopedPointer
<
rapidjson
::
Document
>
doc
(
f
actory
.
create
(
_snakeTilesLocal
));
QtROSJsonFactory
JsonF
actory
;
QScopedPointer
<
rapidjson
::
Document
>
doc
(
JsonF
actory
.
create
(
_snakeTilesLocal
));
auto
&
temp
=
_scenario
.
getOrigin
();
::
GeoPoint3D
origin
(
temp
[
0
],
temp
[
1
],
temp
[
2
]);
QScopedPointer
<
rapidjson
::
Document
>
doc2
(
f
actory
.
create
(
origin
));
QScopedPointer
<
rapidjson
::
Document
>
doc2
(
JsonF
actory
.
create
(
origin
));
cout
.
precision
(
10
);
cout
<<
"Origin"
<<
endl
;
rapidjson
::
OStreamWrapper
out
(
std
::
cout
);
rapidjson
::
Writer
<
rapidjson
::
OStreamWrapper
>
writer
(
out
);
...
...
@@ -628,9 +630,25 @@ bool WimaController::_fetchContainerData()
cout
<<
"Snake Tiles"
<<
endl
;
rapidjson
::
Writer
<
rapidjson
::
OStreamWrapper
>
writer2
(
out
);
doc
->
Accept
(
writer2
);
//
doc->Accept(writer2);
std
::
cout
<<
std
::
endl
<<
std
::
endl
;
QtROSTypeFactory
TypeFactory
;
::
GeoPoint3D
origin_same
;
TypeFactory
.
create
(
*
doc2
.
data
(),
origin_same
);
cout
<<
"Origin2"
<<
endl
;
std
::
cout
<<
origin_same
<<
std
::
endl
;
std
::
cout
<<
"TypeFactory test true: "
;
bool
isSame
=
origin_same
==
origin
;
std
::
cout
<<
isSame
<<
endl
;
::
SnakeTilesLocal
tiles_same
;
TypeFactory
.
create
(
*
doc
.
data
(),
tiles_same
);
}
...
...
src/Wima/WimaController.h
View file @
1de3b8e6
...
...
@@ -26,10 +26,10 @@
#include "snake.h"
#include "WimaControllerDetail.h"
#include "WimaPolygonArray
.h"
#include "snaketile
.h"
#include "
ROSMessageGroup
s.h"
#include "
Polygon2D
.h"
//#include "snaketile
.h"
//#include "Polygon2D
.h"
#include "
SnakeTile
s.h"
#include "
SnakeTilesLocal
.h"
#include "GeoPoint3D.h"
#define CHECK_BATTERY_INTERVAL 1000 // ms
...
...
@@ -327,11 +327,7 @@ private:
SettingsFact
_snakeMinTileArea
;
SettingsFact
_snakeLineDistance
;
SettingsFact
_snakeMinTransectLength
;
typedef
WimaPolygonArray
<
SnakeTile
,
QVector
>
SnakeTiles
;
SnakeTiles
_snakeTiles
;
// tiles
typedef
WimaPolygonArray
<
Polygon2D
,
QVector
,
PolygonArrayGroup
>
SnakeTilesLocal
;
SnakeTiles
_snakeTiles
;
// tiles
SnakeTilesLocal
_snakeTilesLocal
;
// tiles local coordinate system
QVariantList
_snakeTileCenterPoints
;
QList
<
int
>
_snakeProgress
;
// measurement progress
...
...
src/Wima/WimaPolygonArray.h
View file @
1de3b8e6
#pragma once
#include "
ROSMessageType
.h"
#include "
ROS
MessageGroups.h"
#include "
ros_bridge/include/MessageBaseClass
.h"
#include "
ros_bridge/include/
MessageGroups.h"
#include "QmlObjectListModel.h"
#include <QVector>
#include <QString>
typedef
ROSMessageType
<
QString
>
ROSMsg
;
typedef
ROSBridge
::
MessageBaseClass
<
QString
>
ROSMsg
;
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
typedef
MsgGroups
::
EmptyGroup
EmptyGroup
;
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
,
typename
GroupType
=
EmptyGroup
>
class
WimaPolygonArray
:
public
ROSMsg
{
...
...
@@ -19,7 +21,7 @@ public:
,
_polygons
(
other
.
_polygons
),
_dirty
(
true
)
{}
virtual
ROSMessageType
<
QString
>
*
Clone
()
const
override
{
virtual
MessageBaseClass
<
QString
>
*
Clone
()
const
override
{
return
new
WimaPolygonArray
(
*
this
);
}
...
...
src/
Wima/ROS
JsonFactory.h
→
src/
comm/ros_bridge/include/
JsonFactory.h
View file @
1de3b8e6
#ifndef ROSJSONFACTORY_H
#define ROSJSONFACTORY_H
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages.h"
#include "ROSMessageType.h"
#include <QString>
#include "WimaAreaData.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "MessageBaseClass.h"
#include "utilities.h"
#include "ros_bridge/include/
ROS
MessageTraits.h"
#include "ros_bridge/include/MessageTraits.h"
#include "
ROS
MessageGroups.h"
#include "
ros_bridge/include/
MessageGroups.h"
#include "boost/type_traits.hpp"
#include "boost/type_traits/is_base_of.hpp"
namespace
ROSBridge
{
class
StdHeaderPolicy
;
//!
//! \brief The
ROS
JsonFactory class is used to create ROS messages.
//! \brief The JsonFactory class is used to create ROS messages.
//!
//! The
ROS
JsonFactory class is used to create \class rapidjson::Document documents containing ROS messages
//! from classes derived from \class
ROSMessageType
. Each class has a group mark (typedef ... Group) which allows the
//!
ROS
JsonFactory to determine the ROS message type it will create.
template
<
class
HeaderPolicy
=
StdHeaderPolicy
>
class
ROS
JsonFactory
:
public
HeaderPolicy
//! The JsonFactory class is used to create \class rapidjson::Document documents containing ROS messages
//! from classes derived from \class
MessageBaseClass
. Each class has a group mark (typedef ... Group) which allows the
//! JsonFactory to determine the ROS message type it will create.
template
<
class
StringType
=
std
::
string
,
class
HeaderPolicy
=
StdHeaderPolicy
>
class
JsonFactory
:
public
HeaderPolicy
{
typedef
QString
StringType
;
typedef
ROSMessageType
<
StringType
>
ROSMsg
;
typedef
MessageBaseClass
<
StringType
>
ROSMsg
;
public:
ROS
JsonFactory
(){}
JsonFactory
(){}
//!
//! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
//!
//! Creates a \class rapidjson::Document document containing a ROS message from \p msg.
//! A compile-time error will occur, if \p msg belongs to the \struct EmptyGroup or is
//! not derived from \class
ROSMessageType
.
//! \param msg Instance of a \class
ROSMessageType
subclass belonging to a ROSMessageGroup.
//! not derived from \class
MessageBaseClass
.
//! \param msg Instance of a \class
MessageBaseClass
subclass belonging to a ROSMessageGroup.
//! \return rapidjson::Document document containing a ROS message.
template
<
class
T
>
rapidjson
::
Document
*
create
(
const
T
&
msg
){
static_assert
(
boost
::
is_base_of
<
ROSMsg
,
T
>::
value
,
"Type of msg must be derived from
ROSMessageType
."
);
static_assert
(
!::
boost
::
is_same
<
typename
T
::
Group
,
EmptyGroup
>::
value
,
static_assert
(
boost
::
is_base_of
<
ROSMsg
,
T
>::
value
,
"Type of msg must be derived from
MessageBaseClass
."
);
static_assert
(
!::
boost
::
is_same
<
typename
T
::
Group
,
MessageGroups
::
EmptyGroup
>::
value
,
"msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)"
);
//cout << T::Group::label() << endl;
return
_create
(
msg
,
Type2Type
<
typename
T
::
Group
>
());
}
...
...
@@ -62,10 +61,10 @@ private:
// Point32Group
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
Point32Group
>
){
using
namespace
ros_b
ridge
;
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
Point32Group
>
){
using
namespace
ROSB
ridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
Point32
::
toJson
<
_Float32
>
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
Point32
::
toJson
<
_Float32
>
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -76,10 +75,10 @@ private:
// GeoPointGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
GeoPointGroup
>
){
using
namespace
ros_b
ridge
;
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
GeoPointGroup
>
){
using
namespace
ROSB
ridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
GeoPoint
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
GeoPoint
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -90,10 +89,10 @@ private:
// PolygonGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
PolygonGroup
>
){
using
namespace
ros_b
ridge
;
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
PolygonGroup
>
){
using
namespace
ROSB
ridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
Polygon
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
Polygon
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -104,9 +103,9 @@ private:
// PolygonStampedGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
PolygonStampedGroup
>
){
using
namespace
ros_b
ridge
;
using
namespace
ros_b
ridge
::
traits
;
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
PolygonStampedGroup
>
){
using
namespace
ROSB
ridge
;
using
namespace
ROSB
ridge
::
traits
;
typedef
HasMemberHeader
<
U
>
HasHeader
;
return
_createPolygonStamped
(
msg
,
Int2Type
<
HasHeader
::
value
>
());
...
...
@@ -114,9 +113,9 @@ private:
template
<
class
U
,
int
k
>
rapidjson
::
Document
*
_createPolygonStamped
(
const
U
&
msg
,
Int2Type
<
k
>
){
// U has member header(), use integraded header.
using
namespace
ros_b
ridge
;
using
namespace
ROSB
ridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
PolygonStamped
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
PolygonStamped
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -126,10 +125,10 @@ private:
template
<
class
U
>
rapidjson
::
Document
*
_createPolygonStamped
(
const
U
&
msg
,
Int2Type
<
0
>
){
// U has no member header(), generate one on the fly.
using
namespace
ros_b
ridge
;
Header
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
using
namespace
ROSB
ridge
;
JsonMethodes
::
Header
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
PolygonStamped
::
toJson
(
msg
,
header
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
PolygonStamped
::
toJson
(
msg
,
header
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -140,9 +139,9 @@ private:
// PolygonArrayGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
PolygonArrayGroup
>
){
using
namespace
ros_b
ridge
;
using
namespace
ros_b
ridge
::
traits
;
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
PolygonArrayGroup
>
){
using
namespace
ROSB
ridge
;
using
namespace
ROSB
ridge
::
traits
;
typedef
HasMemberHeader
<
U
>
HasHeader
;
return
_createPolygonArray
(
msg
,
Int2Type
<
HasHeader
::
value
>
());
...
...
@@ -150,9 +149,9 @@ private:
template
<
class
U
,
int
k
>
rapidjson
::
Document
*
_createPolygonArray
(
const
U
&
msg
,
Int2Type
<
k
>
){
// U has member header(), use integraded header.
using
namespace
ros_b
ridge
;
using
namespace
ROSB
ridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
PolygonArray
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
PolygonArray
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -162,10 +161,10 @@ private:
template
<
class
U
>
rapidjson
::
Document
*
_createPolygonArray
(
const
U
&
msg
,
Int2Type
<
0
>
){
// U has no member header(), generate one on the fly.
using
namespace
ros_b
ridge
;
Header
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
using
namespace
ROSB
ridge
;
JsonMethodes
::
Header
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
PolygonArray
::
toJson
(
msg
,
header
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
PolygonArray
::
toJson
(
msg
,
header
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -184,8 +183,8 @@ public:
//! \return Returns the header belonging to msg.
//!
template
<
typename
T
>
ros_bridge
::
Header
::
Header
header
(
const
T
&
msg
)
{
return
ros_bridge
::
Header
::
Header
(
++
_seq
,
time
(
msg
),
"/map"
);
ROSBridge
::
JsonMethodes
::
Header
::
Header
header
(
const
T
&
msg
)
{
return
ROSBridge
::
JsonMethodes
::
Header
::
Header
(
++
_seq
,
time
(
msg
),
"/map"
);
}
...
...
@@ -193,12 +192,14 @@ public:
//! \brief time Returns the current time.
//! \return Returns the current time.
template
<
typename
T
>
ros_bridge
::
Time
::
Time
time
(
const
T
&
msg
)
{
ROSBridge
::
JsonMethodes
::
Time
::
Time
time
(
const
T
&
msg
)
{
(
void
)
msg
;
return
ros_bridge
::
Time
::
Time
(
0
,
0
);
return
ROSBridge
::
JsonMethodes
::
Time
::
Time
(
0
,
0
);
}
private:
long
_seq
;
};
#endif // ROSJSONFACTORY_H
}
// end namespace ros_bridge
src/comm/ros_bridge/include/
messag
es.h
→
src/comm/ros_bridge/include/
JsonMethod
es.h
View file @
1de3b8e6
...
...
@@ -10,9 +10,13 @@
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "utilities.h"
#include "ROSMessageTraits.h"
#include "MessageTraits.h"
#include <ostream>
namespace
ros_bridge
{
namespace
ROSBridge
{
namespace
JsonMethodes
{
typedef
std
::
ostream
OStream
;
namespace
Time
{
//! @brief C++ representation of std_msgs/Time
...
...
@@ -43,6 +47,34 @@ namespace Time {
return
true
;
}
template
<
class
TimeType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
TimeType
&
time
)
{
if
(
!
doc
.
HasMember
(
"secs"
)
||
!
doc
[
"secs"
].
IsUint
())
return
false
;
if
(
!
doc
.
HasMember
(
"nsecs"
)
||
!
doc
[
"nsecs"
].
IsUint
())
return
false
;
time
.
setSecs
(
doc
[
"secs"
].
GetUint
());
time
.
setNSecs
(
doc
[
"nsecs"
].
GetUint
());
return
true
;
}
template
<
class
TimeType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
TimeType
&
time
)
{
return
_fromJson
(
doc
,
time
);
}
template
<
class
TimeType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
TimeType
&
time
)
{
return
_fromJson
(
doc
,
time
);
}
}
namespace
Header
{
...
...
@@ -95,10 +127,44 @@ namespace Header {
return
true
;
}
template
<
class
HeaderType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
HeaderType
&
header
)
{
if
(
!
doc
.
HasMember
(
"seq"
)
||
!
doc
[
"seq"
].
IsUint
())
return
false
;
if
(
!
doc
.
HasMember
(
"stamp"
))
return
false
;
if
(
!
doc
.
HasMember
(
"frame_id"
)
||
!
doc
[
"frame_id"
].
IsString
())
return
false
;
header
.
setSeq
(
doc
[
"seq"
].
GetUint
());
decltype
(
header
.
stamp
())
time
;
if
(
!
Time
::
fromJson
(
doc
[
"stamp"
],
time
))
return
false
;
header
.
setStamp
(
time
);
header
.
setFrameId
(
doc
[
"frame_id"
].
GetString
());
return
true
;
}
template
<
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
HeaderType
&
header
)
{
return
_fromJson
(
doc
,
header
);
}
template
<
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
HeaderType
&
header
)
{
return
_fromJson
(
doc
,
header
);
}
}
namespace
Point32
{
using
namespace
ros_b
ridge
::
traits
;
using
namespace
ROSB
ridge
::
traits
;
//! @brief C++ representation of geometry_msgs/Point32.
template
<
typename
FloatType
=
_Float64
>
...
...
@@ -126,39 +192,88 @@ using namespace ros_bridge::traits;
namespace
det
{
//detail
template
<
typename
FloatType
,
class
T
,
typename
V
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Type2Type
<
V
>
)
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has3Components
>
)
{
return
p
.
z
();
}
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
doc
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
((
FloatType
)
p
.
x
()),
allocator
)
;
doc
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
((
FloatType
)
p
.
y
()),
allocator
);
doc
.
AddMember
(
"z"
,
rapidjson
::
Value
().
SetFloat
((
FloatType
)
p
.
z
()),
allocator
);
(
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
<
typename
FloatType
,
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Type2Type
<
Has2Components
>
)
template
<
class
T
>
bool
setZ
(
const
rapidjson
::
Value
&
doc
,
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
doc
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
((
FloatType
)
p
.
x
()),
allocator
);
doc
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
((
FloatType
)
p
.
y
()),
allocator
);
doc
.
AddMember
(
"z"
,
rapidjson
::
Value
().
SetFloat
((
FloatType
)
0
.
0
),
allocator
);
// _point has no member z() -> add 0.
(
void
)
doc
;
(
void
)
p
;
return
true
;
}
}
template
<
typename
FloatType
=
_Float32
,
class
T
>
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
doc
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
(
p
.
x
()),
allocator
);
doc
.
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.
return
det
::
toJson
<
FloatType
>
(
p
,
doc
,
allocator
,
Type2Type
<
Components
>
());
auto
z
=
det
::
getZ
(
p
,
Type2Type
<
Components
>
());
// If T has no member z() replace it by 0.
doc
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
(
z
),
allocator
);
return
true
;
}
template
<
class
PointType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
PointType
&
p
)
{
if
(
!
doc
.
HasMember
(
"x"
)
||
!
doc
[
"x"
].
IsFloat
())
return
false
;
if
(
!
doc
.
HasMember
(
"y"
)
||
!
doc
[
"y"
].
IsFloat
())
return
false
;
if
(
!
doc
.
HasMember
(
"z"
)
||
!
doc
[
"z"
].
IsFloat
())
return
false
;
p
.
setX
(
doc
[
"x"
].
GetFloat
());
p
.
setY
(
doc
[
"y"
].
GetFloat
());
typedef
typename
Select
<
HasMemberSetZ
<
PointType
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
(
void
)
det
::
setZ
(
doc
[
"z"
],
p
,
Type2Type
<
Components
>
());
// If PointType has no member z() discard doc["z"].
return
true
;
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
PointType
&
point
)
{
return
_fromJson
(
doc
,
point
);
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
PointType
&
point
)
{
return
_fromJson
(
doc
,
point
);
}
}
namespace
GeoPoint
{
using
namespace
ros_b
ridge
::
traits
;
using
namespace
ROSB
ridge
::
traits
;
//! @brief C++ representation of geographic_msgs/GeoPoint.
class
GeoPoint
{
...
...
@@ -183,6 +298,23 @@ using namespace ros_bridge::traits;
void
setAltitude
(
_Float64
altitude
)
{
_altitude
=
altitude
;}
bool
operator
==
(
const
GeoPoint
&
p1
)
{
return
(
p1
.
_latitude
==
this
->
_latitude
&&
p1
.
_longitude
==
this
->
_longitude
&&
p1
.
_altitude
==
this
->
_altitude
);
}
bool
operator
!=
(
const
GeoPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
friend
std
::
ostream
&
operator
<<
(
std
::
ostream
&
os
,
const
GeoPoint
&
p
)
{
os
<<
"lat: "
<<
p
.
_latitude
<<
" lon: "
<<
p
.
_longitude
<<
" alt: "
<<
p
.
_altitude
<<
"
\n
"
;
return
os
;
}
private:
_Float64
_latitude
;
_Float64
_longitude
;
...
...
@@ -191,33 +323,80 @@ using namespace ros_bridge::traits;
namespace
det
{
//detail
template
<
class
T
,
typename
V
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Type2Type
<
V
>
)
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
Type2Type
<
Has3Components
>
)
{
doc
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
doc
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
doc
.
AddMember
(
"altitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
altitude
()),
allocator
);
return
p
.
altitude
();
}
return
true
;
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
}
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Type2Type
<
Has2
Components
>
)
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
Type2Type
<
Has3
Components
>
)
{
doc
.
AddMember
(
"latitudex"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
doc
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
doc
.
AddMember
(
"altitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
0
.
0
),
allocator
);
// _point has no member altitude() -> add 0.
p
.
setAltitude
(
doc
.
GetFloat
());
}
return
true
;
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
p
;
}
}
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
doc
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
doc
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
typedef
typename
Select
<
HasMemberAltitude
<
T
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
return
det
::
toJson
(
p
,
doc
,
allocator
,
Type2Type
<
Components
>
());
auto
altitude
=
det
::
getAltitude
(
p
,
Type2Type
<
Components
>
());
// If T has no member altitude() replace it by 0.0;
doc
.
AddMember
(
"altitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
altitude
),
allocator
);
return
true
;
}
template
<
class
PointType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
PointType
&
p
)
{
if
(
!
doc
.
HasMember
(
"latitude"
)
||
!
doc
[
"latitude"
].
IsFloat
())
return
false
;
if
(
!
doc
.
HasMember
(
"longitude"
)
||
!
doc
[
"longitude"
].
IsFloat
())
return
false
;
if
(
!
doc
.
HasMember
(
"altitude"
)
||
!
doc
[
"altitude"
].
IsFloat
())
return
false
;
p
.
setLatitude
(
doc
[
"latitude"
].
GetFloat
());
p
.
setLongitude
(
doc
[
"longitude"
].
GetFloat
());
typedef
typename
Select
<
HasMemberSetAltitude
<
PointType
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
det
::
setAltitude
(
doc
[
"altitude"
],
p
,
Type2Type
<
Components
>
());
// If T has no member altitude() discard doc["altitude"];
return
true
;
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
PointType
&
point
)
{
return
_fromJson
(
doc
,
point
);
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
PointType
&
point
)
{
return
_fromJson
(
doc
,
point
);
}
}
...
...
@@ -248,7 +427,7 @@ namespace Polygon {
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
poly
.
points
().
size
();
++
i
)
{
rapidjson
::
Document
point
(
rapidjson
::
kObjectType
);
if
(
!
Point32
::
toJson
<
_Float32
>
(
poly
.
points
()[
i
],
point
,
allocator
)
)
if
(
!
Point32
::
toJson
(
poly
.
points
()[
i
],
point
,
allocator
)
)
return
false
;
points
.
PushBack
(
point
,
allocator
);
}
...
...
@@ -258,6 +437,33 @@ namespace Polygon {
return
true
;
}
template
<
class
PolygonType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
PolygonType
&
poly
)
{
if
(
!
doc
.
HasMember
(
"points"
)
||
!
doc
[
"points"
].
IsArray
())
return
false
;
const
auto
&
array
=
doc
[
"points"
].
GetArray
();
for
(
long
i
=
0
;
i
<
array
.
Size
();
++
i
){
if
(
!
Point32
::
fromJson
(
array
[
i
],
poly
.
points
()[
i
])
)
return
false
;
}
return
true
;
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
PolygonType
&
poly
)
{
return
_fromJson
(
doc
,
poly
);
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
PolygonType
&
poly
)
{
return
_fromJson
(
doc
,
poly
);
}
}
...
...
@@ -317,10 +523,60 @@ namespace PolygonStamped {
return
true
;
}
namespace
det
{
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
Int2Type
<
1
>
)
{
// polyStamped.setHeader() exists
typedef
decltype
(
polyStamped
.
header
())
HeaderType
;
HeaderType
header
;
bool
ret
=
Header
::
fromJson
(
doc
,
header
);
polyStamped
.
setHeader
(
header
);
return
ret
;
}
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
Int2Type
<
0
>
)
{
// polyStamped.setHeader() does not exists
(
void
)
doc
;
(
void
)
polyStamped
;
return
true
;
}
}
template
<
class
PolygonType
,
class
HeaderType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
PolygonType
&
polyStamped
)
{
if
(
!
doc
.
HasMember
(
"header"
)
)
return
false
;
if
(
!
doc
.
HasMember
(
"polygon"
)
)
return
false
;
typedef
traits
::
HasMemberSetHeader
<
PolygonType
>
HasHeader
;
if
(
!
det
::
setHeader
(
doc
[
"header"
],
polyStamped
,
Int2Type
<
HasHeader
::
value
>
()))
return
false
;
if
(
!
Polygon
::
fromJson
(
doc
[
"polygon"
],
polyStamped
.
polygon
())
)
return
false
;
return
true
;
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
PolygonType
&
poly
)
{
return
_fromJson
(
doc
,
poly
);
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
PolygonType
&
poly
)
{
return
_fromJson
(
doc
,
poly
);
}
}
namespace
PolygonArray
{
using
namespace
ros_b
ridge
::
traits
;
using
namespace
ROSB
ridge
::
traits
;
//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
template
<
class
PointTypeCVR
,
...
...
@@ -332,13 +588,6 @@ using namespace ros_bridge::traits;
typedef
Polygon
<
PointType
>
PolygonType
;
public:
PolygonArray
(){}
//!
//! \brief PolygonArray
//! \param header Header
//! \param polygons Polygons
//! \param labels Labels
//! \param likelihood Header
PolygonArray
(
const
HeaderType
&
header
,
const
ContainerType
<
PolygonType
>
&
polygons
,
const
ContainerType
<
uint32_t
>
&
labels
,
...
...
@@ -403,6 +652,34 @@ using namespace ros_bridge::traits;
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
p
.
polygons
().
size
();
++
i
)
likelyhood
.
PushBack
(
rapidjson
::
Value
().
SetFloat
(
0
),
allocator
);
// use zero!
}
//! \note \p p has member \fn labels().
template
<
class
PolygonArrayType
,
int
k
>
void
setLabels
(
const
rapidjson
::
Value
&
doc
,
PolygonArrayType
&
p
,
Int2Type
<
k
>
){
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
doc
.
Size
();
++
i
)
p
.
labels
().
push_back
(
doc
[
i
]);
}
//! \note \p p has no member \fn labels().
template
<
class
PolygonArrayType
>
void
setLabels
(
const
rapidjson
::
Value
&
doc
,
PolygonArrayType
&
p
,
Int2Type
<
0
>
){
(
void
)
doc
;
(
void
)
p
;
}
//! \note \p p has member \fn likelihood().
template
<
class
PolygonArrayType
,
int
k
>
void
setLikelihood
(
const
rapidjson
::
Value
&
doc
,
PolygonArrayType
&
p
,
Int2Type
<
k
>
){
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
doc
.
Size
();
++
i
)
p
.
likelihood
().
push_back
(
doc
[
i
]);
}
//! \note \p p has no member \fn likelihood().
template
<
class
PolygonArrayType
>
void
setLikelihood
(
const
rapidjson
::
Value
&
doc
,
PolygonArrayType
&
p
,
Int2Type
<
0
>
){
(
void
)
doc
;
(
void
)
p
;
}
}
...
...
@@ -494,7 +771,63 @@ using namespace ros_bridge::traits;
return
true
;
}
template
<
class
PolygonArrayType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
PolygonArrayType
&
p
)
{
if
(
!
doc
.
HasMember
(
"header"
))
return
false
;
if
(
!
doc
.
HasMember
(
"polygons"
)
||
!
doc
[
"polygons"
].
IsArray
()
)
return
false
;
if
(
!
doc
.
HasMember
(
"labels"
)
||
!
doc
[
"labels"
].
IsArray
()
)
return
false
;
if
(
!
doc
.
HasMember
(
"likelihood"
)
||
!
doc
[
"likelihood"
].
IsArray
()
)
return
false
;
typedef
traits
::
HasMemberSetHeader
<
PolygonArrayType
>
HasHeader
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
doc
[
"header"
],
p
,
Int2Type
<
HasHeader
::
value
>
()))
return
false
;
const
auto
&
polygons
=
doc
[
"polygons"
];
p
.
polygons
().
reserve
(
polygons
.
Size
());
for
(
long
i
=
0
;
i
<
polygons
.
Size
();
++
i
)
{
if
(
!
polygons
[
i
].
HasMember
(
"header"
)
)
return
false
;
typedef
traits
::
HasMemberSetHeader
<
PolygonArrayType
>
HasHeader
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
polygons
[
i
][
"header"
],
p
.
polygons
[
i
]().
header
(),
Int2Type
<
HasHeader
::
value
>
()))
return
false
;
if
(
!
Polygon
::
fromJson
(
polygons
[
i
],
p
.
polygons
[
i
].
points
()))
return
false
;
}
typedef
traits
::
HasMemberSetLabels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
setLabels
(
doc
[
"labels"
],
p
,
Int2Type
<
HasLabels
::
value
>
());
typedef
traits
::
HasMemberSetLikelihood
<
PolygonArrayType
>
HasLikelihood
;
PAdetail
::
setLikelihood
(
doc
[
"likelihood"
],
p
,
Int2Type
<
HasLikelihood
::
value
>
());
return
true
;
}
template
<
class
PolygonArrayType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
PolygonArrayType
&
polyArray
)
{
return
_fromJson
(
doc
,
polyArray
);
}
template
<
class
PolygonArrayType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
PolygonArrayType
&
polyArray
)
{
return
_fromJson
(
doc
,
polyArray
);
}
}
}
}
// end namespace JsonMethodes
}
// end namespace ROSBridge
#endif // MESSAGES_H
src/
Wima/ROSMessageType
.h
→
src/
comm/ros_bridge/include/MessageBaseClass
.h
View file @
1de3b8e6
#pragma once
#include "ROSMessageGroups.h"
#include "MessageGroups.h"
namespace
ROSBridge
{
//! @brief Abstract base class for all ROS Messages Types.
//!
//! Abstract base class for all ROS Messages Types. This class defines a basic interface
//! every class must fullfill if it takes advantages of the \class ROSJsonFactory.
//! Every class must define the public typedef Group, which associates it to a message Group (\see
ROS
MessageGroups). The Group type
//! Every class must define the public typedef Group, which associates it to a message Group (\see MessageGroups). The Group type
//! is used by the \class ROSJsonFactory to determine the type of the message it creates. The
//!
ROSMessageType
belongs to the \struct EmptyGroup. Passing a class belonging to the \struct EmptyGroup to the
//!
MessageBaseClass
belongs to the \struct EmptyGroup. Passing a class belonging to the \struct EmptyGroup to the
//! \class ROSJsonFactory will yield an error.
template
<
typename
StringType
>
class
ROSMessageType
{
class
MessageBaseClass
{
public:
typedef
EmptyGroup
Group
;
typedef
MessageGroups
::
EmptyGroup
Group
;
ROSMessageType
()
{};
~
ROSMessageType
()
{};
// Avoid sliceing (copy with ROSMessage::Clone)!
ROSMessageType
(
const
ROSMessageType
&
)
=
delete
;
ROSMessageType
&
operator
=
(
const
ROSMessageType
&
)
=
delete
;
MessageBaseClass
()
{};
~
MessageBaseClass
()
{};
// Avoid sliceing (copy with ROSMessage::Clone
or define subclass operator=
)!
MessageBaseClass
(
const
MessageBaseClass
&
)
=
delete
;
MessageBaseClass
&
operator
=
(
const
MessageBaseClass
&
)
=
delete
;
virtual
ROSMessageType
*
Clone
()
const
=
0
;
virtual
MessageBaseClass
*
Clone
()
const
=
0
;
};
}
src/comm/ros_bridge/include/MessageGroups.h
0 → 100644
View file @
1de3b8e6
#pragma once
#include <string>
namespace
ROSBridge
{
namespace
MessageGroups
{
typedef
std
::
string
StringType
;
template
<
typename
Group
,
typename
SubGroup
,
typename
...
MoreSubGroups
>
struct
MessageGroup
{
static
StringType
label
()
{
return
_label
<
Group
,
SubGroup
,
MoreSubGroups
...
>
();}
template
<
typename
G
,
typename
SubG
,
typename
...
MoreSubG
>
static
StringType
_label
()
{
return
G
::
label
()
+
"/"
+
_label
<
SubG
,
MoreSubG
...
>
();
}
template
<
typename
G
>
static
StringType
_label
()
{
return
G
::
label
();
}
};
//!
//! \note Each class belonging to a ROS message group must derive from \class MessageBaseClass.
namespace
detail
{
//!
//! \brief The EmptyGroup struct is used by the \class MessageBaseClass base class.
//!
//! The EmptyGroup struct is used by the \class MessageBaseClass base class. Passing a class using this
//! group will to the \class JsonFactory will lead to a compile-time error.
struct
EmptyGroup
{
static
StringType
label
()
{
return
""
;}
};
}
struct
GeometryMsgs
{
static
StringType
label
()
{
return
"geometry_msgs"
;}
//!
//! \brief The Point32Group struct is used the mark a class as a ROS Point32 message.
//!
//! The Point32Group struct is used the mark a class as a ROS Point32 message.
struct
Point32Group
{
static
StringType
label
()
{
return
"Point32"
;}
};
//!
//! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
//!
//! The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
struct
GeoPointGroup
{
static
StringType
label
()
{
return
"GeoPoint"
;}
};
//!
//! \brief The PolygonGroup struct is used the mark a class as a ROS Polygon message.
//!
//! The PolygonGroup struct is used the mark a class as a ROS Polygon message.
struct
PolygonGroup
{
static
StringType
label
()
{
return
"Polygon"
;}
};
//!
//! \brief The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
//!
//! The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
struct
PolygonStampedGroup
{
static
StringType
label
()
{
return
"PolygonStamped"
;}
};
};
struct
JSKRecognitionMsgs
{
static
StringType
label
()
{
return
"jsk_recognition_msgs"
;}
//!
//! \brief The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
//!
//! The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
struct
PolygonArrayGroup
{
static
StringType
label
()
{
return
"PolygonArray"
;}
};
};
typedef
MessageGroup
<
detail
::
EmptyGroup
,
detail
::
EmptyGroup
>
EmptyGroup
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
GeometryMsgs
,
MessageGroups
::
GeometryMsgs
::
Point32Group
>
Point32Group
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
GeometryMsgs
,
MessageGroups
::
GeometryMsgs
::
GeoPointGroup
>
GeoPointGroup
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
GeometryMsgs
,
MessageGroups
::
GeometryMsgs
::
PolygonGroup
>
PolygonGroup
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
GeometryMsgs
,
MessageGroups
::
GeometryMsgs
::
PolygonStampedGroup
>
PolygonStampedGroup
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
JSKRecognitionMsgs
,
MessageGroups
::
JSKRecognitionMsgs
::
PolygonArrayGroup
>
PolygonArrayGroup
;
}
// end namespace MessageGroups
}
// end namespace ros_bridge
src/comm/ros_bridge/include/
ROS
MessageTraits.h
→
src/comm/ros_bridge/include/MessageTraits.h
View file @
1de3b8e6
#pragma once
namespace
ros_b
ridge
{
namespace
ROSB
ridge
{
namespace
traits
{
...
...
@@ -17,6 +17,22 @@ class HasMemberZ
template
<
typename
C
>
static
Small
test
(
typeof
(
&
C
::
z
)
)
;
template
<
typename
C
>
static
Big
test
(...);
public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function SetZ.
//!
//! Checks if class T has a member function SetZ. E.g if the class Point has a member function
//! SetZ than HasMemberSetZ<Point>::value is equal to 1 and 0 either.
template
<
typename
T
>
class
HasMemberSetZ
{
typedef
char
Small
;
struct
Big
{
char
x
[
2
];
};
template
<
typename
C
>
static
Small
test
(
typeof
(
&
C
::
SetZ
)
)
;
template
<
typename
C
>
static
Big
test
(...);
public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
...
...
@@ -38,6 +54,23 @@ public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function SetLabels.
//!
//! Checks if class T has a member function SetLabels. E.g if the class T has a member function
//! SetLabels than HasMemberSetLabels<T>::value is equal to 1 and 0 either.
template
<
typename
T
>
class
HasMemberSetLabels
{
typedef
char
Small
;
struct
Big
{
char
x
[
2
];
};
template
<
typename
C
>
static
Small
test
(
typeof
(
&
C
::
SetLabels
)
)
;
template
<
typename
C
>
static
Big
test
(...);
public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function labels.
//!
//! Checks if class T has a member function likelihood. E.g if the class T has a member function
...
...
@@ -55,7 +88,24 @@ public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function labels.
//! @brief Checks if class T has a member function setLikelihood.
//!
//! Checks if class T has a member function setLikelihood. E.g if the class T has a member function
//! setLikelihood than HasMemberSetLikelihood<T>::value is equal to 1 and 0 either.
template
<
typename
T
>
class
HasMemberSetLikelihood
{
typedef
char
Small
;
struct
Big
{
char
x
[
2
];
};
template
<
typename
C
>
static
Small
test
(
typeof
(
&
C
::
setLikelihood
)
)
;
template
<
typename
C
>
static
Big
test
(...);
public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function header.
//!
//! Checks if class T has a member function header. E.g if the class T has a member function
//! header than HasMemberHeader<T>::value is equal to 1 and 0 either.
...
...
@@ -72,6 +122,23 @@ public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function setHeader.
//!
//! Checks if class T has a member function setHeader. E.g if the class T has a member function
//! setHeader than HasMemberSetHeader<T>::value is equal to 1 and 0 either.
template
<
typename
T
>
class
HasMemberSetHeader
{
typedef
char
Small
;
struct
Big
{
char
x
[
2
];
};
template
<
typename
C
>
static
Small
test
(
typeof
(
&
C
::
setHeader
)
)
;
template
<
typename
C
>
static
Big
test
(...);
public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function altitude.
//!
//! Checks if class T has a member function altitude. E.g if the class T has a member function
...
...
@@ -89,6 +156,23 @@ public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Checks if class T has a member function setAltitude.
//!
//! Checks if class T has a member function setAltitude. E.g if the class T has a member function
//! setAltitude than HasMemberSetAltitude<T>::value is equal to 1 and 0 either.
template
<
typename
T
>
class
HasMemberSetAltitude
{
typedef
char
Small
;
struct
Big
{
char
x
[
2
];
};
template
<
typename
C
>
static
Small
test
(
typeof
(
&
C
::
setAltitude
)
)
;
template
<
typename
C
>
static
Big
test
(...);
public:
enum
{
value
=
sizeof
(
test
<
T
>
(
0
))
==
sizeof
(
Small
)
};
};
//! @brief Selects the Type T or U depending on flag and stores it inside Result.
template
<
int
flag
,
typename
T
,
typename
U
>
struct
Select
{
typedef
T
Result
;};
...
...
src/comm/ros_bridge/include/ROSCommunicator.h
0 → 100644
View file @
1de3b8e6
#pragma once
#include "ros_bridge/include/MessageBaseClass.h"
typedef
ROSBridge
::
MessageBaseClass
<
std
::
string
>
ROSMsg
;
class
ROSCommunicator
{
public:
explicit
ROSCommunicator
()
{}
void
send
(
const
ROSMsg
*
msg
);
public
:
virtual
void
receive
(
ROSMsg
*
msg
)
=
0
;
};
src/comm/ros_bridge/include/TypeFactory.h
0 → 100644
View file @
1de3b8e6
#pragma once
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "ros_bridge/include/MessageBaseClass.h"
#include "utilities.h"
#include "ros_bridge/include/MessageTraits.h"
#include "ros_bridge/include/MessageGroups.h"
#include "boost/type_traits.hpp"
#include "boost/type_traits/is_base_of.hpp"
namespace
ROSBridge
{
//!
//! \brief The TypeFactory class is used to create C++ representatives of ROS messages.
//!
//! The TypeFactory class is used to create C++ representatives of ROS messages
//! (classes derived from \class MessageBaseClass) from a \class rapidjson::Document document.
template
<
class
StringType
=
std
::
string
>
class
TypeFactory
{
typedef
MessageBaseClass
<
StringType
>
ROSMsg
;
public:
TypeFactory
(){}
//!
//! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
//!
//! Creates a \class rapidjson::Document document containing a ROS message from \p msg.
//! A compile-time error will occur, if \p msg belongs to the \struct EmptyGroup or is
//! not derived from \class MessageBaseClass.
//! \param msg Instance of a \class MessageBaseClass subclass belonging to a ROSMessageGroup.
//! \return rapidjson::Document document containing a ROS message.
template
<
class
T
>
bool
create
(
const
rapidjson
::
Document
&
doc
,
T
&
type
){
static_assert
(
boost
::
is_base_of
<
ROSMsg
,
T
>::
value
,
"Type of msg must be derived from MessageBaseClass."
);
static_assert
(
!::
boost
::
is_same
<
typename
T
::
Group
,
MessageGroups
::
EmptyGroup
>::
value
,
"msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)"
);
return
_create
(
doc
,
type
,
Type2Type
<
typename
T
::
Group
>
());
}
private:
// ===========================================================================
// EmptyGroup and not implemented Groups
// ===========================================================================
template
<
class
U
,
class
V
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
V
>
){
(
void
)
type
;
(
void
)
doc
;
assert
(
false
);
// Implementation missing for group U::Group!
}
// ===========================================================================
// Point32Group
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
Point32Group
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
Point32
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
// ===========================================================================
// GeoPointGroup
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
GeoPointGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
GeoPoint
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
// ===========================================================================
// PolygonGroup
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
PolygonGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
Polygon
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
// ===========================================================================
// PolygonStampedGroup
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
PolygonStampedGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
PolygonStamped
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
// ===========================================================================
// PolygonArrayGroup
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
PolygonArrayGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
PolygonArray
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
};
}
// end namespace ros_bridge
src/comm/ros_bridge/src/ROSCommunicator.cpp
0 → 100644
View file @
1de3b8e6
#include "ros_bridge/include/ROSCommunicator.h"
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