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
a15105e3
Commit
a15105e3
authored
Jul 09, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
working on ROSBridge::Bride, code not compilable.
parent
1de3b8e6
Changes
17
Hide whitespace changes
Inline
Side-by-side
Showing
17 changed files
with
1020 additions
and
539 deletions
+1020
-539
qgroundcontrol.pro
qgroundcontrol.pro
+6
-0
NemoProgress.h
src/Wima/NemoProgress.h
+7
-0
Polygon2D.h
src/Wima/Polygon2D.h
+24
-12
WimaController.cc
src/Wima/WimaController.cc
+62
-1
WimaController.h
src/Wima/WimaController.h
+0
-2
GenericMessages.h
src/comm/ros_bridge/include/GenericMessages.h
+331
-0
JsonFactory.h
src/comm/ros_bridge/include/JsonFactory.h
+29
-15
JsonMethodes.h
src/comm/ros_bridge/include/JsonMethodes.h
+313
-486
MessageBaseClass.h
src/comm/ros_bridge/include/MessageBaseClass.h
+3
-2
MessageGroups.h
src/comm/ros_bridge/include/MessageGroups.h
+30
-5
PackageBuffer.h
src/comm/ros_bridge/include/PackageBuffer.h
+47
-0
ROSCommunicator.h
src/comm/ros_bridge/include/ROSCommunicator.h
+20
-8
TypeFactory.h
src/comm/ros_bridge/include/TypeFactory.h
+17
-8
CasePacker.cpp
src/comm/ros_bridge/src/CasePacker.cpp
+39
-0
CasePacker.h
src/comm/ros_bridge/src/CasePacker.h
+42
-0
PackageBuffer.h
src/comm/ros_bridge/src/PackageBuffer.h
+45
-0
ROSCommunicator.cpp
src/comm/ros_bridge/src/ROSCommunicator.cpp
+5
-0
No files found.
qgroundcontrol.pro
View file @
a15105e3
...
...
@@ -430,6 +430,7 @@ HEADERS += \
src
/
Snake
/
snake_geometry
.
h
\
src
/
Snake
/
snake_global
.
h
\
src
/
Wima
/
GeoPoint3D
.
h
\
src
/
Wima
/
NemoProgress
.
h
\
src
/
Wima
/
Polygon2D
.
h
\
src
/
Wima
/
PolygonArray
.
h
\
src
/
Wima
/
QtROSJsonFactory
.
h
\
...
...
@@ -472,9 +473,13 @@ HEADERS += \
src
/
Wima
/
testplanimetrycalculus
.
h
\
src
/
Settings
/
WimaSettings
.
h
\
src
/
QmlControls
/
QmlObjectVectorModel
.
h
\
src
/
comm
/
ros_bridge
/
include
/
GenericMessages
.
h
\
src
/
comm
/
ros_bridge
/
include
/
JsonMethodes
.
h
\
src
/
comm
/
ros_bridge
/
include
/
MessageTraits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
PackageBuffer
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TypeFactory
.
h
\
src
/
comm
/
ros_bridge
/
src
/
CasePacker
.
h
\
src
/
comm
/
ros_bridge
/
src
/
PackageBuffer
.
h
\
src
/
comm
/
utilities
.
h
SOURCES
+=
\
src
/
Snake
/
clipper
/
clipper
.
cpp
\
...
...
@@ -482,6 +487,7 @@ SOURCES += \
src
/
Snake
/
snake_geometry
.
cpp
\
src
/
Wima
/
GeoPoint3D
.
cpp
\
src
/
Wima
/
PolygonArray
.
cc
\
src
/
comm
/
ros_bridge
/
src
/
CasePacker
.
cpp
\
src
/
comm
/
ros_bridge
/
src
/
ROSCommunicator
.
cpp
\
src
/
Wima
/
WimaControllerDetail
.
cc
\
src
/
Wima
/
snaketile
.
cpp
\
...
...
src/Wima/NemoProgress.h
0 → 100644
View file @
a15105e3
#pragma once
#include <QVector>
#include "ros_bridge/include/JsonMethodes.h"
namespace
ProgressNS
=
ROSBridge
::
JsonMethodes
::
Progress
;
typedef
ProgressNS
::
GenericProgress
<
long
,
QVector
>
NemoProgress
;
src/Wima/Polygon2D.h
View file @
a15105e3
...
...
@@ -5,27 +5,39 @@
#include "ros_bridge/include/MessageGroups.h"
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/JsonMethodes.h"
namespace
MsgGroupsNS
=
ROSBridge
::
MessageGroups
;
namespace
PolyStampedNS
=
ROSBridge
::
JsonMethodes
::
PolygonStamped
;
typedef
ROSBridge
::
MessageBaseClass
<
QString
>
ROSMsg
;
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
class
Polygon2D
:
public
QPolygonF
,
public
ROSMsg
{
template
<
class
PointType
=
QPointF
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
>
class
Polygon2DTemplate
:
public
ROSMsg
{
//
typedef
ROSBridge
::
JsonMethodes
::
Polygon
::
Polygon
<
PointType
,
ContainerType
>
Poly
;
public:
typedef
MsgGroups
::
PolygonGroup
Group
;
typedef
MsgGroups
NS
::
PolygonStampedGroup
Group
;
// has no header
Polygon2D
(){}
Polygon2D
(
const
Polygon2D
&
other
)
:
QPolygonF
(
other
)
,
ROSMsg
(
){}
Polygon2D
Template
(){}
Polygon2D
Template
(
const
Polygon2DTemplate
&
other
)
:
ROSMsg
(),
_polygon
(
other
.
_polygon
){}
Polygon2D
&
operator
=
(
const
Polygon2D
&
other
)
{
QPolygonF
::
operator
=
(
other
)
;
Polygon2D
Template
&
operator
=
(
const
Polygon2DTemplate
&
other
)
{
this
->
_polygon
=
other
.
_polygon
;
return
*
this
;
}
virtual
Polygon2D
*
Clone
()
const
{
// ROSMsg
return
new
Polygon2D
(
*
this
);
}
const
Poly
&
polygon
()
const
{
return
_polygon
;}
Poly
&
polygon
()
{
return
_polygon
;}
const
Polygon2D
&
points
()
const
{
return
*
this
;}
// ROSMsg
Polygon2D
&
points
()
{
return
*
this
;}
// ROSMsg
const
ContainerType
<
PointType
>
&
path
()
const
{
return
_polygon
.
points
();}
ContainerType
<
PointType
>
&
path
()
{
return
_polygon
.
points
();}
virtual
Polygon2DTemplate
*
Clone
()
const
{
// ROSMsg
return
new
Polygon2DTemplate
(
*
this
);
}
private:
Poly
_polygon
;
};
typedef
Polygon2DTemplate
<>
Polygon2D
;
src/Wima/WimaController.cc
View file @
a15105e3
...
...
@@ -7,6 +7,7 @@
#include "QtROSJsonFactory.h"
#include "QtROSTypeFactory.h"
#include "NemoProgress.h"
#include "time.h"
#include "assert.h"
...
...
@@ -609,7 +610,7 @@ bool WimaController::_fetchContainerData()
Polygon2D
Tile
;
for
(
const
auto
&
vertex
:
tile
.
outer
())
{
QPointF
QVertex
(
vertex
.
get
<
0
>
(),
vertex
.
get
<
1
>
());
Tile
.
append
(
QVertex
);
Tile
.
path
().
append
(
QVertex
);
}
_snakeTilesLocal
.
polygons
().
append
(
Tile
);
}
...
...
@@ -649,7 +650,67 @@ bool WimaController::_fetchContainerData()
TypeFactory
.
create
(
*
doc
.
data
(),
tiles_same
);
Polygon2D
tile1
;
tile1
.
path
().
push_back
(
QPointF
(
1
,
0
));
tile1
.
path
().
push_back
(
QPointF
(
1
,
1
));
tile1
.
path
().
push_back
(
QPointF
(
1
,
2
));
Polygon2D
tile2
;
tile2
.
path
().
push_back
(
QPointF
(
2
,
0
));
tile2
.
path
().
push_back
(
QPointF
(
2
,
1
));
tile2
.
path
().
push_back
(
QPointF
(
2
,
2
));
Polygon2D
tile3
;
tile3
.
path
().
push_back
(
QPointF
(
3
,
0
));
tile3
.
path
().
push_back
(
QPointF
(
3
,
1
));
tile3
.
path
().
push_back
(
QPointF
(
3
,
2
));
SnakeTilesLocal
tilesSmall
;
tilesSmall
.
polygons
().
push_back
(
tile1
);
tilesSmall
.
polygons
().
push_back
(
tile2
);
tilesSmall
.
polygons
().
push_back
(
tile3
);
QScopedPointer
<
rapidjson
::
Document
>
jsonTileSmall
(
JsonFactory
.
create
(
tilesSmall
));
SnakeTilesLocal
tilesSmallSame
;
TypeFactory
.
create
(
*
jsonTileSmall
.
data
(),
tilesSmallSame
);
QScopedPointer
<
rapidjson
::
Document
>
jsonTileSmallSame
(
JsonFactory
.
create
(
tilesSmallSame
));
cout
<<
"Snake Tiles small"
<<
endl
;
rapidjson
::
Writer
<
rapidjson
::
OStreamWrapper
>
writer4
(
out
);
jsonTileSmall
->
Accept
(
writer4
);
std
::
cout
<<
std
::
endl
<<
std
::
endl
;
cout
<<
"Snake Tiles small same"
<<
endl
;
rapidjson
::
Writer
<
rapidjson
::
OStreamWrapper
>
writer5
(
out
);
jsonTileSmallSame
->
Accept
(
writer5
);
std
::
cout
<<
std
::
endl
<<
std
::
endl
;
// progress
NemoProgress
progress
;
progress
.
progress
().
push_back
(
1
);
progress
.
progress
().
push_back
(
2
);
progress
.
progress
().
push_back
(
3
);
progress
.
progress
().
push_back
(
4
);
progress
.
progress
().
push_back
(
13
);
progress
.
progress
().
push_back
(
600
);
QScopedPointer
<
rapidjson
::
Document
>
jsonProgress
(
JsonFactory
.
create
(
progress
));
NemoProgress
progressSame
;
TypeFactory
.
create
(
*
jsonProgress
.
data
(),
progressSame
);
QScopedPointer
<
rapidjson
::
Document
>
jsonProgressSame
(
JsonFactory
.
create
(
progressSame
));
cout
<<
"Snake Tiles small"
<<
endl
;
rapidjson
::
Writer
<
rapidjson
::
OStreamWrapper
>
writer6
(
out
);
jsonProgress
->
Accept
(
writer6
);
std
::
cout
<<
std
::
endl
<<
std
::
endl
;
cout
<<
"Snake Tiles small same"
<<
endl
;
rapidjson
::
Writer
<
rapidjson
::
OStreamWrapper
>
writer7
(
out
);
jsonProgressSame
->
Accept
(
writer7
);
std
::
cout
<<
std
::
endl
<<
std
::
endl
;
}
...
...
src/Wima/WimaController.h
View file @
a15105e3
...
...
@@ -26,8 +26,6 @@
#include "snake.h"
#include "WimaControllerDetail.h"
//#include "snaketile.h"
//#include "Polygon2D.h"
#include "SnakeTiles.h"
#include "SnakeTilesLocal.h"
#include "GeoPoint3D.h"
...
...
src/comm/ros_bridge/include/GenericMessages.h
0 → 100644
View file @
a15105e3
#pragma once
#include <iostream>
#include <vector>
#include "boost/type_traits/remove_cv_ref.hpp"
#include "ros_bridge/include/MessageBaseClass.h"
namespace
ROSBridge
{
//!@brief Namespace containing ROS message generics.
namespace
GenericMessages
{
//!@brief Namespace containing ROS std_msgs generics.
typedef
std
::
ostream
OStream
;
namespace
StdMsgs
{
//! @brief C++ representation of std_msgs/Time
class
Time
{
public:
Time
()
:
_secs
(
0
),
_nsecs
(
0
)
{}
Time
(
uint32_t
secs
,
uint32_t
nsecs
)
:
_secs
(
secs
),
_nsecs
(
nsecs
)
{}
uint32_t
secs
()
const
{
return
_secs
;}
uint32_t
nSecs
()
const
{
return
_nsecs
;}
void
setSecs
(
uint32_t
secs
)
{
_secs
=
secs
;}
void
setNSecs
(
uint32_t
nsecs
)
{
_nsecs
=
nsecs
;}
private:
uint32_t
_secs
;
uint32_t
_nsecs
;
};
//! @brief C++ representation of std_msgs/Header
class
Header
{
public:
Header
()
:
_seq
(
0
),
_stamp
(
Time
()),
_frameId
(
""
)
{}
Header
(
uint32_t
seq
,
const
Time
&
stamp
,
const
std
::
string
&
frame_id
)
:
_seq
(
seq
)
,
_stamp
(
stamp
)
,
_frameId
(
frame_id
)
{}
uint32_t
seq
()
const
{
return
_seq
;};
const
Time
&
stamp
()
const
{
return
_stamp
;};
const
std
::
string
&
frameId
()
const
{
return
_frameId
;};
Time
&
stamp
()
{
return
_stamp
;};
std
::
string
&
frameId
()
{
return
_frameId
;};
void
setSeq
(
uint32_t
seq
)
{
_seq
=
seq
;}
void
setStamp
(
const
Time
&
stamp
)
{
_stamp
=
stamp
;}
void
setFrameId
(
const
std
::
string
&
frameId
)
{
_frameId
=
frameId
;}
private:
uint32_t
_seq
;
Time
_stamp
;
std
::
string
_frameId
;
};
}
//StdMsgs
//!@brief Namespace containing ROS geometry_msgs generics.
namespace
GeometryMsgs
{
// ==============================================================================
// class GenericPoint
// ==============================================================================
//! @brief C++ representation of a geometry_msgs/Point32
template
<
typename
FloatType
=
_Float64
>
class
GenericPoint
:
public
ROSBridge
::
MessageBaseClass
{
public:
typedef
ROSBridge
::
MessageGroups
::
Point32Group
Group
;
GenericPoint
()
:
ROSBridge
::
MessageBaseClass
(){}
GenericPoint
(
FloatType
x
,
FloatType
y
,
FloatType
z
)
:
ROSBridge
::
MessageBaseClass
(),
_x
(
x
),
_y
(
y
),
_z
(
z
){}
FloatType
x
()
const
{
return
_x
;}
FloatType
y
()
const
{
return
_y
;}
FloatType
z
()
const
{
return
_z
;}
void
setX
(
FloatType
x
)
{
_x
=
x
;}
void
setY
(
FloatType
y
)
{
_y
=
y
;}
void
setZ
(
FloatType
z
)
{
_z
=
z
;}
bool
operator
==
(
const
GenericPoint
&
p1
)
{
return
(
p1
.
_x
==
this
->
_x
&&
p1
.
_y
==
this
->
_y
&&
p1
.
_z
==
this
->
_z
);
}
bool
operator
!=
(
const
GenericPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericPoint
&
p
)
{
os
<<
"x: "
<<
p
.
_x
<<
" y: "
<<
p
.
_y
<<
" z: "
<<
p
.
_z
;
return
os
;
}
GenericPoint
*
Clone
()
const
override
{
return
new
GenericPoint
(
*
this
);}
private:
FloatType
_x
;
FloatType
_y
;
FloatType
_z
;
};
typedef
GenericPoint
<>
Point
;
typedef
GenericPoint
<
_Float32
>
Point32
;
// ==============================================================================
// class GenericPolygon
// ==============================================================================
//! @brief C++ representation of geometry_msgs/Polygon
template
<
class
PointTypeCVR
,
template
<
class
,
class
...
>
class
ContainerType
=
std
::
vector
>
class
GenericPolygon
:
public
ROSBridge
::
MessageBaseClass
{
typedef
typename
boost
::
remove_cv_ref_t
<
PointTypeCVR
>
PointType
;
typedef
ROSBridge
::
MessageBaseClass
Base
;
public:
typedef
ROSBridge
::
MessageGroups
::
PolygonGroup
Group
;
GenericPolygon
()
:
Base
()
{}
GenericPolygon
(
const
GenericPolygon
&
poly
)
:
Base
(),
_points
(
poly
.
points
()){}
const
ContainerType
<
PointType
>
&
points
()
const
{
return
_points
;}
ContainerType
<
PointType
>
&
points
()
{
return
_points
;}
GenericPolygon
*
Clone
()
const
override
{
return
new
GenericPolygon
(
*
this
);}
private:
ContainerType
<
PointType
>
_points
;
};
typedef
GenericPolygon
<
Point
>
Polygon
;
// ==============================================================================
// class GenericPolygonStamped
// ==============================================================================
using
namespace
ROSBridge
::
GenericMessages
::
StdMsgs
;
//! @brief C++ representation of geometry_msgs/PolygonStamped
template
<
class
PolygonType
=
Polygon
,
class
HeaderType
=
Header
>
class
GenericPolygonStamped
:
public
ROSBridge
::
MessageBaseClass
{
typedef
PolygonType
Polygon
;
typedef
ROSBridge
::
MessageBaseClass
Base
;
public:
typedef
ROSBridge
::
MessageGroups
::
PolygonStampedGroup
Group
;
GenericPolygonStamped
()
:
Base
()
{};
GenericPolygonStamped
(
const
GenericPolygonStamped
&
other
)
:
Base
()
,
_header
(
other
.
header
())
,
_polygon
(
other
.
polygon
())
{}
GenericPolygonStamped
(
const
HeaderType
&
header
,
Polygon
&
polygon
)
:
Base
()
,
_header
(
header
)
,
_polygon
(
polygon
)
{}
const
HeaderType
&
header
()
const
{
return
_header
;}
const
Polygon
&
polygon
()
const
{
return
_polygon
;}
HeaderType
&
header
()
{
return
_header
;}
Polygon
&
polygon
()
{
return
_polygon
;}
GenericPolygonStamped
*
Clone
()
const
override
{
return
new
GenericPolygonStamped
(
*
this
);}
private:
HeaderType
_header
;
Polygon
_polygon
;
};
typedef
GenericPolygonStamped
<>
PolygonStamped
;
}
// namespace GeometryMsgs
//!@brief Namespace containing ROS geographic_msgs generics.
namespace
GeographicMsgs
{
// ==============================================================================
// class GenericGeoPoint
// ==============================================================================
//! @brief C++ representation of geographic_msgs/GeoPoint.
class
GeoPoint
:
public
ROSBridge
::
MessageBaseClass
{
typedef
ROSBridge
::
MessageBaseClass
Base
;
public:
typedef
ROSBridge
::
MessageGroups
::
GeoPointGroup
Group
;
GeoPoint
()
:
Base
()
,
_latitude
(
0
)
,
_longitude
(
0
)
,
_altitude
(
0
)
{}
GeoPoint
(
const
GeoPoint
&
other
)
:
Base
()
,
_latitude
(
other
.
latitude
())
,
_longitude
(
other
.
longitude
())
,
_altitude
(
other
.
altitude
())
{}
GeoPoint
(
_Float64
latitude
,
_Float64
longitude
,
_Float64
altitude
)
:
Base
()
,
_latitude
(
latitude
)
,
_longitude
(
longitude
)
,
_altitude
(
altitude
)
{}
_Float64
latitude
()
const
{
return
_latitude
;}
_Float64
longitude
()
const
{
return
_longitude
;}
_Float64
altitude
()
const
{
return
_altitude
;}
void
setLatitude
(
_Float64
latitude
)
{
_latitude
=
latitude
;}
void
setLongitude
(
_Float64
longitude
)
{
_longitude
=
longitude
;}
void
setAltitude
(
_Float64
altitude
)
{
_altitude
=
altitude
;}
GeoPoint
*
Clone
()
const
override
{
return
new
GeoPoint
(
*
this
);}
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
OStream
&
operator
<<
(
OStream
&
os
,
const
GeoPoint
&
p
)
{
os
<<
"lat: "
<<
p
.
_latitude
<<
" lon: "
<<
p
.
_longitude
<<
" alt: "
<<
p
.
_altitude
;
return
os
;
}
private:
_Float64
_latitude
;
_Float64
_longitude
;
_Float64
_altitude
;
};
}
// namespace GeographicMsgs
//!@brief Namespace containing ROS jsk_recognition_msgs generics.
namespace
JSKRecognitionMsgs
{
using
namespace
ROSBridge
::
GenericMessages
::
StdMsgs
;
using
namespace
ROSBridge
::
GenericMessages
::
GeometryMsgs
;
// ==============================================================================
// class GenericPolygonArray
// ==============================================================================
//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
template
<
class
PolygonType
=
Polygon
,
template
<
class
,
class
...
>
class
ContainerType
=
std
::
vector
,
class
HeaderType
=
Header
,
class
IntType
=
long
,
class
FloatType
=
double
>
class
GenericPolygonArray
:
ROSBridge
::
MessageBaseClass
{
typedef
ROSBridge
::
MessageBaseClass
Base
;
public:
typedef
ROSBridge
::
MessageGroups
::
PolygonArrayGroup
Group
;
GenericPolygonArray
()
:
Base
()
{}
GenericPolygonArray
(
const
GenericPolygonArray
&
other
)
:
Base
()
,
_header
(
other
.
header
())
,
_polygons
(
other
.
polygons
())
,
_labels
(
other
.
labels
())
,
_likelihood
(
other
.
likelihood
())
{}
GenericPolygonArray
(
const
HeaderType
&
header
,
const
ContainerType
<
PolygonType
>
&
polygons
,
const
ContainerType
<
IntType
>
&
labels
,
const
ContainerType
<
FloatType
>
&
likelihood
)
:
Base
()
,
_header
(
header
)
,
_polygons
(
polygons
)
,
_labels
(
labels
)
,
_likelihood
(
likelihood
)
{}
const
HeaderType
&
header
()
const
{
return
_header
;}
HeaderType
&
header
()
{
return
_header
;}
const
ContainerType
<
PolygonType
>
&
polygons
()
const
{
return
_polygons
;}
ContainerType
<
PolygonType
>
&
polygons
()
{
return
_polygons
;}
const
ContainerType
<
IntType
>
&
labels
()
const
{
return
_labels
;}
ContainerType
<
IntType
>
&
labels
()
{
return
_labels
;}
const
ContainerType
<
FloatType
>
&
likelyhood
()
const
{
return
_likelihood
;}
ContainerType
<
FloatType
>
&
likelyhood
()
{
return
_likelihood
;}
GenericPolygonArray
*
Clone
()
const
override
{
return
new
GenericPolygonArray
(
*
this
);}
private:
HeaderType
_header
;
ContainerType
<
PolygonType
>
_polygons
;
ContainerType
<
IntType
>
_labels
;
ContainerType
<
FloatType
>
_likelihood
;
};
typedef
GenericPolygonArray
<>
PolygonArray
;
}
// namespace JSKRecognitionMsgs
//!@brief Namespace containing ROS nemo_msgs generics.
namespace
NemoMsgs
{
// ==============================================================================
// class GenericProgress
// ==============================================================================
//! @brief C++ representation of nemo_msgs/Progress message
template
<
class
IntType
=
long
,
template
<
class
,
class
...
>
class
ContainterType
=
std
::
vector
>
class
GenericProgress
{
public:
GenericProgress
(){}
GenericProgress
(
const
ContainterType
<
IntType
>
&
progress
)
:
_progress
(
progress
){}
const
ContainterType
<
IntType
>
&
progress
(
void
)
const
{
return
_progress
;}
ContainterType
<
IntType
>
&
progress
(
void
)
{
return
_progress
;}
private:
ContainterType
<
IntType
>
_progress
;
};
typedef
GenericProgress
<>
Progress
;
}
// namespace NemoMsgs
}
// namespace GenericMessages
}
// namespace ROSBridge
src/comm/ros_bridge/include/JsonFactory.h
View file @
a15105e3
...
...
@@ -4,9 +4,10 @@
#include "ros_bridge/include/JsonMethodes.h"
#include "MessageBaseClass.h"
#include "utilities.h"
#include "ros_bridge/include/MessageTraits.h"
#include "ros_bridge/include/MessageTraits.h"
#include "ros_bridge/include/MessageGroups.h"
#include "ros_bridge/include/GenericMessages.h"
#include "boost/type_traits.hpp"
#include "boost/type_traits/is_base_of.hpp"
...
...
@@ -21,13 +22,13 @@ class StdHeaderPolicy;
//! 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
>
template
<
class
HeaderPolicy
=
StdHeaderPolicy
>
class
JsonFactory
:
public
HeaderPolicy
{
typedef
MessageBaseClass
<
StringType
>
ROSMsg
;
typedef
MessageBaseClass
ROSMsg
;
public:
JsonFactory
(){}
JsonFactory
()
:
HeaderPolicy
()
{}
//!
//! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
...
...
@@ -64,7 +65,7 @@ private:
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
Point32Group
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
Point32
::
toJson
<
_Float32
>
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
Point32
::
toJson
<
_Float32
>
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -78,7 +79,7 @@ private:
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
GeoPointGroup
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
GeoPoint
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
Geo
graphicMsgs
::
Geo
Point
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -92,7 +93,7 @@ private:
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
PolygonGroup
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
Polygon
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
Polygon
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -115,7 +116,7 @@ private:
rapidjson
::
Document
*
_createPolygonStamped
(
const
U
&
msg
,
Int2Type
<
k
>
){
// U has member header(), use integraded header.
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
PolygonStamped
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
PolygonStamped
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -128,7 +129,7 @@ private:
using
namespace
ROSBridge
;
JsonMethodes
::
Header
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
PolygonStamped
::
toJson
(
msg
,
header
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
PolygonStamped
::
toJson
(
msg
.
polygon
()
,
header
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -151,7 +152,7 @@ private:
rapidjson
::
Document
*
_createPolygonArray
(
const
U
&
msg
,
Int2Type
<
k
>
){
// U has member header(), use integraded header.
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
PolygonArray
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
JSKRecognitionMsg
::
PolygonArray
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
...
...
@@ -164,17 +165,30 @@ private:
using
namespace
ROSBridge
;
JsonMethodes
::
Header
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
PolygonArray
::
toJson
(
msg
,
header
,
*
doc
,
doc
->
GetAllocator
());
bool
ret
=
JsonMethodes
::
JSKRecognitionMsg
::
PolygonArray
::
toJson
(
msg
,
header
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
// ===========================================================================
// ProgressGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
ProgressGroup
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
NemoMsgs
::
Progress
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
};
class
StdHeaderPolicy
{
namespace
StdMsgs
=
ROSBridge
::
GenericMessages
::
StdMsgs
;
public:
StdHeaderPolicy
()
:
_seq
(
-
1
){};
...
...
@@ -183,8 +197,8 @@ public:
//! \return Returns the header belonging to msg.
//!
template
<
typename
T
>
ROSBridge
::
JsonMethodes
::
Header
::
Header
header
(
const
T
&
msg
)
{
return
ROSBridge
::
JsonMethodes
::
Header
::
Header
(
++
_seq
,
time
(
msg
),
"/map"
);
StdMsgs
::
Header
header
(
const
T
&
msg
)
{
return
StdMsgs
::
Header
(
++
_seq
,
time
(
msg
),
"/map"
);
}
...
...
@@ -192,9 +206,9 @@ public:
//! \brief time Returns the current time.
//! \return Returns the current time.
template
<
typename
T
>
ROSBridge
::
JsonMethodes
::
Time
::
Time
time
(
const
T
&
msg
)
{
StdMsgs
::
Time
time
(
const
T
&
msg
)
{
(
void
)
msg
;
return
ROSBridge
::
JsonMethodes
::
Time
::
Time
(
0
,
0
);
return
StdMsgs
::
Time
(
0
,
0
);
}
private:
long
_seq
;
...
...
src/comm/ros_bridge/include/JsonMethodes.h
View file @
a15105e3
...
...
@@ -14,111 +14,62 @@
#include <ostream>
namespace
ROSBridge
{
namespace
JsonMethodes
{
typedef
std
::
ostream
OStream
;
namespace
Time
{
//! @brief C++ representation of std_msgs/Time
class
Time
{
public:
Time
()
:
_secs
(
0
),
_nsecs
(
0
)
{}
Time
(
uint32_t
secs
,
uint32_t
nsecs
)
:
_secs
(
secs
),
_nsecs
(
nsecs
)
{}
uint32_t
secs
()
const
{
return
_secs
;}
uint32_t
nSecs
()
const
{
return
_nsecs
;}
void
setSecs
(
uint32_t
secs
)
{
_secs
=
secs
;}
void
setNSecs
(
uint32_t
nsecs
)
{
_nsecs
=
nsecs
;}
private:
uint32_t
_secs
;
uint32_t
_nsecs
;
};
//! @brief Namespace containing methodes for Json generation.
namespace
JsonMethodes
{
//! @brief Namespace containing methodes for std_msgs generation.
namespace
StdMsgs
{
//! @brief Namespace containing methodes for std_msgs/Time message generation.
namespace
Time
{
template
<
class
TimeType
>
bool
toJson
(
const
TimeType
&
time
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
doc
.
AddMember
(
"secs"
,
rapidjson
::
Value
().
SetUint
((
uint32_t
)
time
.
secs
()),
allocator
);
doc
.
AddMember
(
"nsecs"
,
rapidjson
::
Value
().
SetUint
((
uint32_t
)
time
.
nSecs
()),
allocator
);
value
.
AddMember
(
"secs"
,
rapidjson
::
Value
().
SetUint
((
uint32_t
)
time
.
secs
()),
allocator
);
value
.
AddMember
(
"nsecs"
,
rapidjson
::
Value
().
SetUint
((
uint32_t
)
time
.
nSecs
()),
allocator
);
return
true
;
}
template
<
class
TimeType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
template
<
class
TimeType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
TimeType
&
time
)
{
if
(
!
doc
.
HasMember
(
"secs"
)
||
!
doc
[
"secs"
].
IsUint
())
if
(
!
value
.
HasMember
(
"secs"
)
||
!
value
[
"secs"
].
IsUint
()){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"nsecs"
)
||
!
doc
[
"nsecs"
].
IsUint
())
}
if
(
!
value
.
HasMember
(
"nsecs"
)
||
!
value
[
"nsecs"
].
IsUint
()){
assert
(
false
);
return
false
;
time
.
setSecs
(
doc
[
"secs"
].
GetUint
());
time
.
setNSecs
(
doc
[
"nsecs"
].
GetUint
());
}
time
.
setSecs
(
value
[
"secs"
].
GetUint
());
time
.
setNSecs
(
value
[
"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
);
}
}
}
// Time
//! @brief Namespace containing methodes for std_msgs/Header message generation.
namespace
Header
{
//! @brief C++ representation of std_msgs/Header
class
Header
{
public:
Header
()
:
_seq
(
0
),
_stamp
(
Time
::
Time
()),
_frameId
(
""
)
{}
Header
(
uint32_t
seq
,
const
Time
::
Time
&
stamp
,
const
std
::
string
&
frame_id
)
:
_seq
(
seq
)
,
_stamp
(
stamp
)
,
_frameId
(
frame_id
)
{}
uint32_t
seq
()
const
{
return
_seq
;};
const
Time
::
Time
&
stamp
()
const
{
return
_stamp
;};
const
std
::
string
&
frameId
()
const
{
return
_frameId
;};
Time
::
Time
&
stamp
()
{
return
_stamp
;};
std
::
string
&
frameId
()
{
return
_frameId
;};
void
setSeq
(
uint32_t
seq
)
{
_seq
=
seq
;}
void
setStamp
(
const
Time
::
Time
&
stamp
)
{
_stamp
=
stamp
;}
void
setFrameId
(
const
std
::
string
&
frameId
)
{
_frameId
=
frameId
;}
private:
uint32_t
_seq
;
Time
::
Time
_stamp
;
std
::
string
_frameId
;
};
template
<
class
HeaderType
>
bool
toJson
(
const
HeaderType
&
header
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
doc
.
AddMember
(
"seq"
,
rapidjson
::
Value
().
SetUint
((
uint32_t
)
header
.
seq
()),
allocator
);
value
.
AddMember
(
"seq"
,
rapidjson
::
Value
().
SetUint
((
uint32_t
)
header
.
seq
()),
allocator
);
rapidjson
::
Document
stamp
(
rapidjson
::
kObjectType
);
if
(
!
Time
::
toJson
(
header
.
stamp
(),
doc
,
allocator
))
return
false
;
doc
.
AddMember
(
"stamp"
,
stamp
,
allocator
);
rapidjson
::
Value
stamp
(
rapidjson
::
kObjectType
);
if
(
!
Time
::
toJson
(
header
.
stamp
(),
stamp
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"stamp"
,
stamp
,
allocator
);
doc
.
AddMember
(
"frame_id"
,
value
.
AddMember
(
"frame_id"
,
rapidjson
::
Value
().
SetString
(
header
.
frameId
().
data
(),
header
.
frameId
().
length
(),
allocator
),
...
...
@@ -127,69 +78,43 @@ namespace Header {
return
true
;
}
template
<
class
HeaderType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
template
<
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
HeaderType
&
header
)
{
if
(
!
doc
.
HasMember
(
"seq"
)
||
!
doc
[
"seq"
].
IsUint
())
if
(
!
value
.
HasMember
(
"seq"
)
||
!
value
[
"seq"
].
IsUint
()){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"stamp"
))
}
if
(
!
value
.
HasMember
(
"stamp"
)){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"frame_id"
)
||
!
doc
[
"frame_id"
].
IsString
())
}
if
(
!
value
.
HasMember
(
"frame_id"
)
||
!
value
[
"frame_id"
].
IsString
()){
assert
(
false
);
return
false
;
header
.
setSeq
(
doc
[
"seq"
].
GetUint
());
}
header
.
setSeq
(
value
[
"seq"
].
GetUint
());
decltype
(
header
.
stamp
())
time
;
if
(
!
Time
::
fromJson
(
doc
[
"stamp"
],
time
))
if
(
!
Time
::
fromJson
(
value
[
"stamp"
],
time
)){
assert
(
false
);
return
false
;
}
header
.
setStamp
(
time
);
header
.
setFrameId
(
doc
[
"frame_id"
].
GetString
());
header
.
setFrameId
(
value
[
"frame_id"
].
GetString
());
return
true
;
}
}
// Header
}
// StdMsgs
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
);
}
}
//! @brief Namespace containing methodes for geometry_msgs generation.
namespace
GeometryMsgs
{
//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
namespace
Point32
{
using
namespace
ROSBridge
::
traits
;
//! @brief C++ representation of geometry_msgs/Point32.
template
<
typename
FloatType
=
_Float64
>
class
Point
{
public:
Point
(
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
;}
private:
FloatType
_x
;
FloatType
_y
;
FloatType
_z
;
};
typedef
Point
<>
Point64
;
typedef
Point
<
_Float32
>
Point32
;
namespace
det
{
//detail
template
<
class
T
>
...
...
@@ -222,104 +147,177 @@ using namespace ROSBridge::traits;
}
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
doc
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
(
p
.
x
()),
allocator
);
doc
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
(
p
.
y
()),
allocator
);
value
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
(
p
.
x
()),
allocator
);
value
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
(
p
.
y
()),
allocator
);
typedef
typename
Select
<
HasMemberZ
<
T
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
auto
z
=
det
::
getZ
(
p
,
Type2Type
<
Components
>
());
// If T has no member z() replace it by 0.
doc
.
AddMember
(
"y
"
,
rapidjson
::
Value
().
SetFloat
(
z
),
allocator
);
value
.
AddMember
(
"z
"
,
rapidjson
::
Value
().
SetFloat
(
z
),
allocator
);
return
true
;
}
template
<
class
PointType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
if
(
!
doc
.
HasMember
(
"x"
)
||
!
doc
[
"x"
].
IsFloat
())
if
(
!
value
.
HasMember
(
"x"
)
||
!
value
[
"x"
].
IsFloat
()){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"y"
)
||
!
doc
[
"y"
].
IsFloat
())
}
if
(
!
value
.
HasMember
(
"y"
)
||
!
value
[
"y"
].
IsFloat
()){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"z"
)
||
!
doc
[
"z"
].
IsFloat
())
}
if
(
!
value
.
HasMember
(
"z"
)
||
!
value
[
"z"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setX
(
doc
[
"x"
].
GetFloat
());
p
.
setY
(
doc
[
"y"
].
GetFloat
());
p
.
setX
(
value
[
"x"
].
GetFloat
());
p
.
setY
(
value
[
"y"
].
GetFloat
());
typedef
typename
Select
<
HasMemberSetZ
<
PointType
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
(
void
)
det
::
setZ
(
doc
[
"z"
],
p
,
Type2Type
<
Components
>
());
// If PointType has no member z() discard doc["z"].
(
void
)
det
::
setZ
(
value
[
"z"
],
p
,
Type2Type
<
Components
>
());
// If PointType has no member z() discard doc["z"].
return
true
;
}
}
// Point32
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
PointType
&
point
)
{
return
_fromJson
(
doc
,
point
);
//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
namespace
Polygon
{
template
<
class
PolygonType
>
bool
toJson
(
const
PolygonType
&
poly
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Value
points
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
poly
.
points
().
size
();
++
i
)
{
rapidjson
::
Document
point
(
rapidjson
::
kObjectType
);
if
(
!
Point32
::
toJson
(
poly
.
points
()[
i
],
point
,
allocator
)
){
assert
(
false
);
return
false
;
}
points
.
PushBack
(
point
,
allocator
);
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
PointType
&
point
)
{
return
_fromJson
(
doc
,
point
);
value
.
AddMember
(
"points"
,
points
,
allocator
);
return
true
;
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonType
&
poly
)
{
if
(
!
value
.
HasMember
(
"points"
)
||
!
value
[
"points"
].
IsArray
()){
assert
(
false
);
return
false
;
}
const
auto
&
jsonArray
=
value
[
"points"
].
GetArray
();
poly
.
points
().
reserve
(
jsonArray
.
Size
());
typedef
decltype
(
poly
.
points
()[
0
])
PointTypeCVR
;
typedef
typename
boost
::
remove_cv
<
typename
boost
::
remove_reference
<
PointTypeCVR
>::
type
>::
type
PointType
;
for
(
long
i
=
0
;
i
<
jsonArray
.
Size
();
++
i
){
PointType
pt
;
if
(
!
Point32
::
fromJson
(
jsonArray
[
i
],
pt
)
){
assert
(
false
);
return
false
;
}
poly
.
points
().
push_back
(
std
::
move
(
pt
));
}
return
true
;
}
}
// namespace Polygon
//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation.
namespace
PolygonStamped
{
using
namespace
ROSBridge
::
JsonMethodes
::
StdMsgs
;
template
<
class
PolyStamped
>
bool
toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
return
toJson
(
polyStamped
.
polygon
(),
polyStamped
.
header
(),
value
,
allocator
);
}
namespace
GeoPoint
{
using
namespace
ROSBridge
::
traits
;
template
<
class
PolygonType
,
class
HeaderType
>
bool
toJson
(
const
PolygonType
&
poly
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
Header
::
toJson
(
h
,
header
,
allocator
)){
assert
(
false
);
return
false
;
}
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
Polygon
::
toJson
(
poly
,
polygon
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"header"
,
header
,
allocator
);
value
.
AddMember
(
"polygon"
,
polygon
,
allocator
);
//! @brief C++ representation of geographic_msgs/GeoPoint.
class
GeoPoint
{
public:
GeoPoint
()
:
_latitude
(
0
)
,
_longitude
(
0
)
,
_altitude
(
0
)
{}
GeoPoint
(
_Float64
latitude
,
_Float64
longitude
,
_Float64
altitude
)
:
_latitude
(
latitude
)
,
_longitude
(
longitude
)
,
_altitude
(
altitude
)
{}
_Float64
latitude
()
const
{
return
_latitude
;}
_Float64
longitude
()
const
{
return
_longitude
;}
_Float64
altitude
()
const
{
return
_altitude
;}
void
setLatitude
(
_Float64
latitude
)
{
_latitude
=
latitude
;}
void
setLongitude
(
_Float64
longitude
)
{
_longitude
=
longitude
;}
void
setAltitude
(
_Float64
altitude
)
{
_altitude
=
altitude
;}
bool
operator
==
(
const
GeoPoint
&
p1
)
{
return
(
p1
.
_latitude
==
this
->
_latitude
&&
p1
.
_longitude
==
this
->
_longitude
&&
p1
.
_altitude
==
this
->
_altitude
);
}
return
true
;
}
bool
operator
!=
(
const
GeoPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
namespace
det
{
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
Int2Type
<
1
>
)
{
// polyStamped.setHeader() exists
typedef
decltype
(
polyStamped
.
header
())
HeaderTypeCVR
;
typedef
typename
boost
::
remove_cv
<
typename
boost
::
remove_reference
<
HeaderTypeCVR
>::
type
>::
type
HeaderType
;
HeaderType
header
;
bool
ret
=
Header
::
fromJson
(
doc
,
header
);
polyStamped
.
header
()
=
header
;
return
ret
;
}
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
Int2Type
<
0
>
)
{
// polyStamped.setHeader() does not exists
(
void
)
doc
;
(
void
)
polyStamped
;
return
true
;
}
}
// namespace det
template
<
class
PolygonType
,
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonType
&
polyStamped
)
{
if
(
!
value
.
HasMember
(
"header"
)
){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"polygon"
)
){
assert
(
false
);
return
false
;
}
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
;
_Float64
_altitude
;
};
typedef
traits
::
HasMemberSetHeader
<
PolygonType
>
HasHeader
;
if
(
!
det
::
setHeader
(
value
[
"header"
],
polyStamped
,
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
if
(
!
Polygon
::
fromJson
(
value
[
"polygon"
],
polyStamped
.
polygon
())
){
assert
(
false
);
return
false
;
}
return
true
;
}
}
// namespace PolygonStamped
}
// namespace GeometryMsgs
//! @brief Namespace containing methodes for geographic_msgs generation.
namespace
GeographicMsgs
{
//! @brief Namespace containing methodes for geographic_msgs/GeoPoint message generation.
namespace
GeoPoint
{
using
namespace
ROSBridge
::
traits
;
namespace
det
{
//detail
...
...
@@ -348,279 +346,60 @@ using namespace ROSBridge::traits;
(
void
)
doc
;
(
void
)
p
;
}
}
}
// namespace det
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
doc
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
doc
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
value
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
value
.
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.
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
);
value
.
AddMember
(
"altitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
altitude
),
allocator
);
return
true
;
}
template
<
class
PointType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
if
(
!
doc
.
HasMember
(
"latitude"
)
||
!
doc
[
"latitude"
].
IsFloat
())
if
(
!
value
.
HasMember
(
"latitude"
)
||
!
value
[
"latitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"longitude"
)
||
!
doc
[
"longitude"
].
IsFloat
())
}
if
(
!
value
.
HasMember
(
"longitude"
)
||
!
value
[
"longitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"altitude"
)
||
!
doc
[
"altitude"
].
IsFloat
())
}
if
(
!
value
.
HasMember
(
"altitude"
)
||
!
value
[
"altitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setLatitude
(
doc
[
"latitude"
].
GetFloat
());
p
.
setLongitude
(
doc
[
"longitude"
].
GetFloat
());
p
.
setLatitude
(
value
[
"latitude"
].
GetFloat
());
p
.
setLongitude
(
value
[
"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"];
det
::
setAltitude
(
value
[
"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
);
}
}
namespace
Polygon
{
//! @brief C++ representation of geometry_msgs/Polygon
template
<
class
PointTypeCVR
,
template
<
class
,
class
...
>
class
ContainerType
=
std
::
vector
>
class
Polygon
{
typedef
typename
boost
::
remove_cv
<
typename
boost
::
remove_reference
<
PointTypeCVR
>::
type
>::
type
PointType
;
public:
Polygon
(){}
Polygon
(
const
Polygon
&
poly
)
:
_points
(
poly
.
points
()){}
const
ContainerType
<
PointType
>
&
points
()
const
{
return
_points
;}
ContainerType
<
PointType
>
&
points
()
{
return
_points
;}
private:
ContainerType
<
PointType
>
_points
;
};
template
<
class
PolygonType
>
bool
toJson
(
const
PolygonType
&
poly
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Value
points
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
poly
.
points
().
size
();
++
i
)
{
rapidjson
::
Document
point
(
rapidjson
::
kObjectType
);
if
(
!
Point32
::
toJson
(
poly
.
points
()[
i
],
point
,
allocator
)
)
return
false
;
points
.
PushBack
(
point
,
allocator
);
}
doc
.
AddMember
(
"points"
,
points
,
allocator
);
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
);
}
}
namespace
PolygonStamped
{
//! @brief C++ representation of geometry_msgs/PolygonStamped
template
<
class
PointTypeCVR
,
template
<
class
,
class
...
>
class
PolygonType
=
Polygon
::
Polygon
,
class
HeaderType
=
Header
::
Header
>
class
PolygonStamped
{
typedef
typename
boost
::
remove_cv
<
typename
boost
::
remove_reference
<
PointTypeCVR
>::
type
>::
type
PointType
;
typedef
PolygonType
<
PointType
>
Polygon
;
public:
PolygonStamped
(){};
PolygonStamped
(
const
HeaderType
&
header
,
Polygon
&
polygon
)
:
_header
(
header
)
,
_polygon
(
polygon
)
{}
const
HeaderType
&
header
()
const
{
return
_header
;}
const
Polygon
&
polygon
()
const
{
return
_polygon
;}
HeaderType
&
header
()
{
return
_header
;}
Polygon
&
polygon
()
{
return
_polygon
;}
void
setHeader
(
const
HeaderType
&
header
)
{
_header
=
header
;}
void
setPolygon
(
const
Polygon
&
polygon
)
{
_polygon
=
polygon
;
}
private:
HeaderType
_header
;
Polygon
_polygon
;
};
template
<
class
PolyStamped
>
bool
toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
return
toJson
(
polyStamped
.
polygon
(),
polyStamped
.
header
(),
doc
,
allocator
);
}
template
<
class
PolygonType
,
class
HeaderType
>
bool
toJson
(
const
PolygonType
&
poly
,
const
HeaderType
&
h
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
Header
::
toJson
(
h
,
header
,
allocator
))
return
false
;
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
Polygon
::
toJson
(
poly
,
polygon
,
allocator
))
return
false
;
doc
.
AddMember
(
"header"
,
header
,
allocator
);
doc
.
AddMember
(
"polygon"
,
polygon
,
allocator
);
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
;
}
// GeoPoint
}
// GeographicMsgs
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
);
}
}
//! @brief Namespace containing methodes for jsk_recognition_msgs generation.
namespace
JSKRecognitionMsgs
{
//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
namespace
PolygonArray
{
using
namespace
ROSBridge
::
traits
;
//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
template
<
class
PointTypeCVR
,
template
<
class
,
class
...
>
class
Polygon
=
Polygon
::
Polygon
,
template
<
class
,
class
...
>
class
ContainerType
=
std
::
vector
,
class
HeaderType
=
Header
::
Header
>
class
PolygonArray
{
typedef
typename
boost
::
remove_cv
<
typename
boost
::
remove_reference
<
PointTypeCVR
>::
type
>::
type
PointType
;
typedef
Polygon
<
PointType
>
PolygonType
;
public:
PolygonArray
(){}
PolygonArray
(
const
HeaderType
&
header
,
const
ContainerType
<
PolygonType
>
&
polygons
,
const
ContainerType
<
uint32_t
>
&
labels
,
const
ContainerType
<
_Float32
>
&
likelihood
)
:
_header
(
header
)
,
_polygons
(
polygons
)
,
_labels
(
labels
)
,
_likelihood
(
likelihood
)
{}
const
HeaderType
&
header
()
const
{
return
_header
;}
HeaderType
&
header
()
{
return
_header
;}
const
ContainerType
<
PolygonType
>
&
polygons
()
const
{
return
_polygons
;}
ContainerType
<
PolygonType
>
&
polygons
()
{
return
_polygons
;}
const
ContainerType
<
uint32_t
>
&
labels
()
const
{
return
_labels
;}
ContainerType
<
uint32_t
>
&
labels
()
{
return
_labels
;}
const
ContainerType
<
_Float32
>
&
likelyhood
()
const
{
return
_likelihood
;}
ContainerType
<
_Float32
>
&
likelyhood
()
{
return
_likelihood
;}
void
setHeader
(
const
HeaderType
&
header
)
{
_header
=
&
header
;}
void
setPolygons
(
ContainerType
<
PolygonType
>
&
polygon
)
{
_polygons
=
polygon
;
}
void
setLabels
(
const
ContainerType
<
uint32_t
>
&
labels
)
{
_labels
=
labels
;}
void
setLikelihood
(
const
ContainerType
<
_Float32
>
&
likelihood
)
{
_likelihood
=
likelihood
;}
private:
HeaderType
_header
;
ContainerType
<
PolygonType
>
_polygons
;
ContainerType
<
uint32_t
>
_labels
;
ContainerType
<
_Float32
>
_likelihood
;
};
using
namespace
ROSBridge
::
JsonMethodes
::
StdMsgs
;
using
namespace
ROSBridge
::
JsonMethodes
::
GeometryMsgs
;
namespace
PAdetail
{
//! Helper functions to generate Json entries for labels and likelihood.
...
...
@@ -695,33 +474,37 @@ using namespace ROSBridge::traits;
//! \note If this function is called p.polygons[i] (entries implement the the PolygonStampedGroup interface)
//! must contain a header.
template
<
class
PolygonArrayType
>
bool
toJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
bool
toJson
(
const
PolygonArrayType
&
p
Array
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
Header
::
toJson
(
p
.
header
(),
header
,
allocator
))
if
(
Header
::
toJson
(
pArray
.
header
(),
header
,
allocator
)){
assert
(
false
);
return
false
;
doc
.
AddMember
(
"header"
,
header
,
allocator
);
}
value
.
AddMember
(
"header"
,
header
,
allocator
);
rapidjson
::
Value
polygons
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
p
.
polygons
().
size
();
++
i
){
for
(
unsigned
long
i
=
0
;
i
<
p
Array
.
polygons
().
size
();
++
i
){
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
PolygonStamped
::
toJson
(
p
.
polygons
[
i
],
polygon
,
allocator
))
if
(
!
PolygonStamped
::
toJson
(
pArray
.
polygons
()[
i
],
polygon
,
allocator
)){
assert
(
false
);
return
false
;
}
polygons
.
PushBack
(
polygon
,
allocator
);
}
doc
.
AddMember
(
"polygons"
,
polygons
,
allocator
);
value
.
AddMember
(
"polygons"
,
polygons
,
allocator
);
rapidjson
::
Value
labels
(
rapidjson
::
kArrayType
);
typedef
HasMemberLabels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
labelsToJson
(
p
,
labels
,
allocator
,
Int2Type
<
HasLabels
::
value
>
());
doc
.
AddMember
(
"labels"
,
labels
,
allocator
);
PAdetail
::
labelsToJson
(
p
Array
,
labels
,
allocator
,
Int2Type
<
HasLabels
::
value
>
());
value
.
AddMember
(
"labels"
,
labels
,
allocator
);
rapidjson
::
Value
likelihood
(
rapidjson
::
kArrayType
);
typedef
HasMemberLikelihood
<
PolygonArrayType
>
HasLikelihood
;
PAdetail
::
likelihoodToJson
(
p
,
likelihood
,
allocator
,
Int2Type
<
HasLikelihood
::
value
>
());
doc
.
AddMember
(
"likelihood"
,
likelihood
,
allocator
);
PAdetail
::
likelihoodToJson
(
p
Array
,
likelihood
,
allocator
,
Int2Type
<
HasLikelihood
::
value
>
());
value
.
AddMember
(
"likelihood"
,
likelihood
,
allocator
);
return
true
;
}
...
...
@@ -740,94 +523,138 @@ using namespace ROSBridge::traits;
//! \note If this function is called the headers in p.polygons[i] (entries implement the the PolygonStampedGroup interface)
//! are ignored.
template
<
class
PolygonArrayType
,
class
HeaderType
>
bool
toJson
(
const
PolygonArrayType
&
p
,
const
HeaderType
&
h
,
rapidjson
::
Document
&
doc
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
bool
toJson
(
const
PolygonArrayType
&
p
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
Header
::
toJson
(
h
,
header
,
allocator
))
if
(
!
Header
::
toJson
(
h
,
header
,
allocator
)){
assert
(
false
);
return
false
;
doc
.
AddMember
(
"header"
,
header
,
allocator
);
}
value
.
AddMember
(
"header"
,
header
,
allocator
);
rapidjson
::
Value
polygons
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)(
p
.
polygons
().
size
());
++
i
){
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
PolygonStamped
::
toJson
(
p
.
polygons
()[
i
],
h
,
polygon
,
allocator
))
if
(
!
PolygonStamped
::
toJson
(
p
.
polygons
()[
i
].
polygon
(),
h
,
polygon
,
allocator
)){
assert
(
false
);
return
false
;
}
polygons
.
PushBack
(
polygon
,
allocator
);
}
doc
.
AddMember
(
"polygons"
,
polygons
,
allocator
);
value
.
AddMember
(
"polygons"
,
polygons
,
allocator
);
rapidjson
::
Value
labels
(
rapidjson
::
kArrayType
);
typedef
HasMemberLabels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
labelsToJson
(
p
,
labels
,
allocator
,
Int2Type
<
HasLabels
::
value
>
());
doc
.
AddMember
(
"labels"
,
labels
,
allocator
);
value
.
AddMember
(
"labels"
,
labels
,
allocator
);
rapidjson
::
Value
likelihood
(
rapidjson
::
kArrayType
);
typedef
HasMemberLikelihood
<
PolygonArrayType
>
HasLikelihood
;
PAdetail
::
likelihoodToJson
(
p
,
likelihood
,
allocator
,
Int2Type
<
HasLikelihood
::
value
>
());
doc
.
AddMember
(
"likelihood"
,
likelihood
,
allocator
);
value
.
AddMember
(
"likelihood"
,
likelihood
,
allocator
);
return
true
;
}
template
<
class
PolygonArrayType
,
class
JsonType
>
bool
_fromJson
(
const
JsonType
&
doc
,
PolygonArrayType
&
p
)
template
<
class
PolygonArrayType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonArrayType
&
p
)
{
if
(
!
doc
.
HasMember
(
"header"
))
if
(
!
value
.
HasMember
(
"header"
)){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"polygons"
)
||
!
doc
[
"polygons"
].
IsArray
()
)
}
if
(
!
value
.
HasMember
(
"polygons"
)
||
!
value
[
"polygons"
].
IsArray
()
){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"labels"
)
||
!
doc
[
"labels"
].
IsArray
()
)
}
if
(
!
value
.
HasMember
(
"labels"
)
||
!
value
[
"labels"
].
IsArray
()
){
assert
(
false
);
return
false
;
if
(
!
doc
.
HasMember
(
"likelihood"
)
||
!
doc
[
"likelihood"
].
IsArray
()
)
}
if
(
!
value
.
HasMember
(
"likelihood"
)
||
!
value
[
"likelihood"
].
IsArray
()
){
assert
(
false
);
return
false
;
}
typedef
traits
::
HasMemberSetHeader
<
PolygonArrayType
>
HasHeader
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
doc
[
"header"
],
p
,
Int2Type
<
HasHeader
::
value
>
()))
typedef
traits
::
HasMemberHeader
<
PolygonArrayType
>
HasHeader
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
value
[
"header"
],
p
,
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
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"
)
)
const
auto
&
polyStampedJson
=
value
[
"polygons"
];
p
.
polygons
().
reserve
(
polyStampedJson
.
Size
());
typedef
decltype
(
p
.
polygons
()[
0
])
PolyStampedCVR
;
typedef
typename
boost
::
remove_cv
<
typename
boost
::
remove_reference
<
PolyStampedCVR
>::
type
>::
type
PolyStamped
;
for
(
long
i
=
0
;
i
<
polyStampedJson
.
Size
();
++
i
)
{
if
(
!
polyStampedJson
[
i
].
HasMember
(
"header"
)
){
assert
(
false
);
return
false
;
typedef
traits
::
HasMemberSetHeader
<
PolygonArrayType
>
HasHeader
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
polygons
[
i
][
"header"
],
p
.
polygons
[
i
]().
header
(),
Int2Type
<
HasHeader
::
value
>
()))
}
PolyStamped
polyStamped
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
polyStampedJson
[
i
][
"header"
],
polyStamped
,
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
if
(
!
Polygon
::
fromJson
(
polygons
[
i
],
p
.
polygons
[
i
].
points
()))
if
(
!
Polygon
::
fromJson
(
polyStampedJson
[
i
][
"polygon"
],
polyStamped
.
polygon
())){
assert
(
false
);
return
false
;
}
}
typedef
traits
::
HasMemberSetLabels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
setLabels
(
doc
[
"labels"
],
p
,
Int2Type
<
HasLabels
::
value
>
());
p
.
polygons
().
push_back
(
std
::
move
(
polyStamped
))
;
}
typedef
traits
::
HasMember
SetLikelihood
<
PolygonArrayType
>
HasLikelihood
;
PAdetail
::
setL
ikelihood
(
doc
[
"likelihood"
],
p
,
Int2Type
<
HasLikelihood
::
value
>
());
typedef
traits
::
HasMember
Labels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
setL
abels
(
value
[
"labels"
],
p
,
Int2Type
<
HasLabels
::
value
>
());
typedef
traits
::
HasMemberLikelihood
<
PolygonArrayType
>
HasLikelihood
;
PAdetail
::
setLikelihood
(
value
[
"likelihood"
],
p
,
Int2Type
<
HasLikelihood
::
value
>
());
return
true
;
}
template
<
class
PolygonArrayType
>
bool
fromJson
(
const
rapidjson
::
Value
&
doc
,
PolygonArrayType
&
polyArray
)
}
// end namespace PolygonArray
}
// namespace JSKRekognitionMsgs
//! @brief Namespace containing methodes for nemo_msgs generation.
namespace
NemoMsgs
{
//! @brief Namespace containing methodes for nemo_msgs/Progress generation.
namespace
Progress
{
template
<
class
ProgressType
>
bool
toJson
(
const
ProgressType
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
return
_fromJson
(
doc
,
polyArray
);
rapidjson
::
Value
progressJson
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
p
.
progress
().
size
();
++
i
){
progressJson
.
PushBack
(
rapidjson
::
Value
().
SetInt
(
std
::
int8_t
(
p
.
progress
()[
i
])),
allocator
);
}
value
.
AddMember
(
"progress"
,
progressJson
,
allocator
);
}
template
<
class
PolygonArrayType
>
bool
fromJson
(
const
rapidjson
::
Document
&
doc
,
PolygonArrayType
&
polyArray
)
template
<
class
ProgressType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
ProgressType
&
p
)
{
return
_fromJson
(
doc
,
polyArray
);
}
if
(
!
value
.
HasMember
(
"progress"
)
||
!
value
[
"progress"
].
IsArray
()){
assert
(
false
);
return
false
;
}
}
}
// end namespace JsonMethodes
}
// end namespace ROSBridge
unsigned
long
sz
=
value
.
Size
();
p
.
progress
().
reserve
(
sz
);
for
(
unsigned
long
i
=
0
;
i
<
sz
;
++
i
)
p
.
progress
().
push_back
(
value
[
i
]);
}
}
// namespace Progress
}
// namespace NemoMsgs
}
// namespace JsonMethodes
}
// namespace ROSBridge
#endif // MESSAGES_H
src/comm/ros_bridge/include/MessageBaseClass.h
View file @
a15105e3
...
...
@@ -12,7 +12,6 @@ namespace ROSBridge {
//! is used by the \class ROSJsonFactory to determine the type of the message it creates. 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
MessageBaseClass
{
public:
typedef
MessageGroups
::
EmptyGroup
Group
;
...
...
@@ -25,5 +24,7 @@ public:
virtual
MessageBaseClass
*
Clone
()
const
=
0
;
};
typedef
MessageBaseClass
MsgBase
;
}
// namespace ROSBridge
}
src/comm/ros_bridge/include/MessageGroups.h
View file @
a15105e3
...
...
@@ -10,12 +10,20 @@ typedef std::string StringType;
template
<
typename
Group
,
typename
SubGroup
,
typename
...
MoreSubGroups
>
struct
MessageGroup
{
static
StringType
label
()
{
return
_labe
l
<
Group
,
SubGroup
,
MoreSubGroups
...
>
();}
static
StringType
messageNameFull
()
{
return
_ful
l
<
Group
,
SubGroup
,
MoreSubGroups
...
>
();}
template
<
typename
G
,
typename
SubG
,
typename
...
MoreSubG
>
static
StringType
_
label
()
{
return
G
::
label
()
+
"/"
+
_labe
l
<
SubG
,
MoreSubG
...
>
();
}
static
StringType
_
full
()
{
return
G
::
label
()
+
"/"
+
_ful
l
<
SubG
,
MoreSubG
...
>
();
}
template
<
typename
G
>
static
StringType
_label
()
{
return
G
::
label
();
}
static
StringType
_full
()
{
return
G
::
label
();
}
static
StringType
messageNameLast
()
{
return
_last
<
Group
,
SubGroup
,
MoreSubGroups
...
>
();}
template
<
typename
G
,
typename
SubG
,
typename
...
MoreSubG
>
static
StringType
_last
()
{
return
_last
<
SubG
,
MoreSubG
...
>
();
}
template
<
typename
G
>
static
StringType
_last
()
{
return
G
::
label
();
}
};
//!
...
...
@@ -76,14 +84,27 @@ struct JSKRecognitionMsgs {
static
StringType
label
()
{
return
"jsk_recognition_msgs"
;}
//!
//! \brief The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
//! \brief The PolygonArrayGroup struct is used the mark a class as a ROS
jsk_recognition_msgs/
PolygonArray message.
//!
//! 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
jsk_recognition_msgs/
PolygonArray message.
struct
PolygonArrayGroup
{
static
StringType
label
()
{
return
"PolygonArray"
;}
};
};
struct
NemoMsgs
{
static
StringType
label
()
{
return
"nemo_msgs"
;}
//!
//! \brief The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message.
//!
//! The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message.
struct
ProgressGroup
{
static
StringType
label
()
{
return
"Progress"
;}
};
};
typedef
MessageGroup
<
detail
::
EmptyGroup
,
detail
::
EmptyGroup
>
...
...
@@ -109,6 +130,10 @@ typedef MessageGroups::MessageGroup<MessageGroups::JSKRecognitionMsgs,
MessageGroups
::
JSKRecognitionMsgs
::
PolygonArrayGroup
>
PolygonArrayGroup
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
NemoMsgs
,
MessageGroups
::
NemoMsgs
::
ProgressGroup
>
ProgressGroup
;
}
// end namespace MessageGroups
}
// end namespace ros_bridge
src/comm/ros_bridge/include/PackageBuffer.h
0 → 100644
View file @
a15105e3
#pragma once
#pragma once
#include "boost/lockfree/queue.hpp"
#include <functional>
namespace
ROSBridge
{
namespace
Bridge
{
namespace
lf
=
::
boost
::
lockfree
;
template
<
class
T
>
class
PackageBuffer
{
public:
PackageBuffer
();
void
push
(
T
t
)
{
buffer
.
push
(
t
);
if
(
_pushCallback
)
_pushCallback
();
}
T
pop
()
{
T
temp
=
buffer
.
pop
();
if
(
_popCallback
)
_popCallback
();
return
temp
;
}
void
setPushCallback
(
std
::
function
<
void
(
void
)
>
pushCallback
)
{
_pushCallback
=
pushCallback
;
}
void
setPopCallback
(
std
::
function
<
void
(
void
)
>
popCallback
)
{
_popCallback
=
popCallback
;
}
bool
empty
()
const
{
return
buffer
.
empty
();}
private:
lf
::
queue
<
T
>
buffer
;
std
::
function
<
void
(
void
)
>
_popCallback
;
std
::
function
<
void
(
void
)
>
_pushCallback
;
};
}
// namespace Communicator
}
// namespace
src/comm/ros_bridge/include/ROSCommunicator.h
View file @
a15105e3
#pragma once
#include "ros_bridge/
include/MessageBaseClass
.h"
#include "ros_bridge/
rapidjson/include/rapidjson/document
.h"
typedef
ROSBridge
::
MessageBaseClass
<
std
::
string
>
ROSMsg
;
#include <memory>
#include <tuple>
class
ROSCommunicator
#include "boost/lockfree/queue.hpp"
#include "ros_bridge/include/
namespace
ROSBridge
{
namespace
lf
=
::
boost
::
lockfree
;
class
Communicator
{
typedef
std
::
unique_ptr
<
rapidjson
::
Document
>
UniqueJsonPtr
;
typedef
std
::
tuple
<
UniqueJsonPtr
,
std
::
string
>
MsgTopicHashPair
;
public:
explicit
ROSCommunicator
()
{}
void
send
(
const
ROSMsg
*
msg
);
explicit
Communicator
()
{}
void
send
(
UniqueJsonPtr
&
msg
);
void
start
();
void
stop
();
virtual
UniqueJsonPtr
receive
()
=
0
;
public
:
virtual
void
receive
(
ROSMsg
*
msg
)
=
0
;
private:
lf
::
queue
<
MsgTopicPair
>
_transmittBuffer
;
lf
::
queue
<
MsgTopicPair
>
_receiveBuffer
;
};
}
src/comm/ros_bridge/include/TypeFactory.h
View file @
a15105e3
#pragma once
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/JsonMethodes.h"
...
...
@@ -19,10 +18,9 @@ namespace ROSBridge {
//!
//! 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
;
typedef
MessageBaseClass
ROSMsg
;
public:
TypeFactory
(){}
...
...
@@ -61,7 +59,7 @@ private:
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
Point32Group
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
Point32
::
fromJson
(
doc
,
type
);
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
Point32
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
...
...
@@ -72,7 +70,7 @@ private:
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
GeoPointGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
GeoPoint
::
fromJson
(
doc
,
type
);
bool
ret
=
JsonMethodes
::
Geo
graphicMsgs
::
Geo
Point
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
...
...
@@ -83,7 +81,7 @@ private:
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
PolygonGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
Polygon
::
fromJson
(
doc
,
type
);
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
Polygon
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
...
...
@@ -94,7 +92,7 @@ private:
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
PolygonStampedGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
PolygonStamped
::
fromJson
(
doc
,
type
);
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
PolygonStamped
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
...
...
@@ -105,7 +103,18 @@ private:
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
PolygonArrayGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
PolygonArray
::
fromJson
(
doc
,
type
);
bool
ret
=
JsonMethodes
::
JSKRecognitionMsgs
::
PolygonArray
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
// ===========================================================================
// ProgressGroup
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
ProgressGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
NemoMsgs
::
Progress
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
...
...
src/comm/ros_bridge/src/CasePacker.cpp
0 → 100644
View file @
a15105e3
#include "CasePacker.h"
const
char
*
ROSBridge
::
CasePacker
::
topicKey
=
"topic"
;
const
char
*
ROSBridge
::
CasePacker
::
messageTypeKey
=
"messageType"
;
ROSBridge
::
CasePacker
::
CasePacker
()
{
}
void
ROSBridge
::
CasePacker
::
_addTag
(
JsonDoc
&
doc
,
const
char
*
topic
,
const
char
*
messageType
)
{
using
namespace
ROSBridge
;
using
namespace
rapidjson
;
{
// add topic
rapidjson
::
Value
key
(
CasePacker
::
topicKey
,
doc
.
GetAllocator
());
rapidjson
::
Value
value
(
topic
,
doc
.
GetAllocator
());
doc
.
AddMember
(
key
,
value
,
doc
.
GetAllocator
());
}
// add messageType
rapidjson
::
Value
key
(
CasePacker
::
messageTypeKey
,
doc
.
GetAllocator
());
rapidjson
::
Value
value
(
messageType
,
doc
.
GetAllocator
());
doc
.
AddMember
(
key
,
value
,
doc
.
GetAllocator
());
}
void
ROSBridge
::
CasePacker
::
_removeTag
(
JsonDoc
&
doc
)
{
using
namespace
ROSBridge
;
using
namespace
rapidjson
;
if
(
doc
.
HasMember
(
CasePacker
::
topicKey
)
)
doc
.
RemoveMember
(
CasePacker
::
topicKey
);
if
(
doc
.
HasMember
(
CasePacker
::
messageTypeKey
)
)
doc
.
RemoveMember
(
CasePacker
::
messageTypeKey
);
}
src/comm/ros_bridge/src/CasePacker.h
0 → 100644
View file @
a15105e3
#pragma once
#include "ros_bridge/include/MessageBaseClass.h"
#include <memory>
#include "rapidjson/include/rapidjson/document.h"
namespace
ROSBridge
{
class
CasePacker
{
typedef
rapidjson
::
Document
JsonDoc
;
typedef
std
::
unique_ptr
<
JsonDoc
>
UniqueJsonPtr
;
public:
CasePacker
();
struct
MessageTag
{
char
*
topic
;
char
*
messagType
;
};
typedef
MessageTag
Tag
;
template
<
class
T
>
void
packAndSend
(
const
T
&
msg
,
const
char
*
topic
);
const
Tag
&
showTag
();
template
<
class
T
>
void
unpack
(
T
&
msg
);
protected:
void
_addTag
(
JsonDoc
&
doc
,
const
char
*
topic
,
const
char
*
messageType
);
void
_removeTag
(
JsonDoc
&
doc
);
static
const
char
*
topicKey
;
static
const
char
*
messageTypeKey
;
private:
Tag
_tag
;
};
}
src/comm/ros_bridge/src/PackageBuffer.h
0 → 100644
View file @
a15105e3
#pragma once
#include "boost/lockfree/queue.hpp"
#include <functional>
namespace
ROSBridge
{
namespace
Bridge
{
namespace
lf
=
::
boost
::
lockfree
;
template
<
class
T
>
class
PackageBuffer
{
public:
PackageBuffer
();
void
push
(
T
t
)
{
buffer
.
push
(
t
);
if
(
_pushCallback
)
_pushCallback
();
}
T
pop
()
{
T
temp
=
buffer
.
pop
();
if
(
_popCallback
)
_popCallback
();
return
temp
;
}
void
setPushCallback
(
std
::
function
<
void
(
void
)
>
pushCallback
)
{
_pushCallback
=
pushCallback
;
}
void
setPopCallback
(
std
::
function
<
void
(
void
)
>
popCallback
)
{
_popCallback
=
popCallback
;
}
bool
empty
()
const
{
return
buffer
.
empty
();}
private:
lf
::
queue
<
T
>
buffer
;
std
::
function
<
void
(
void
)
>
_popCallback
;
std
::
function
<
void
(
void
)
>
_pushCallback
;
};
}
// namespace Communicator
}
// namespace
src/comm/ros_bridge/src/ROSCommunicator.cpp
View file @
a15105e3
#include "ros_bridge/include/ROSCommunicator.h"
void
ROSBridge
::
Communicator
::
send
(
ROSBridge
::
Communicator
::
UniqueJsonPtr
&
msg
)
{
if
(
!
msg
->
HasMember
(
""
))
}
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