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
32d51139
Commit
32d51139
authored
Aug 26, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Plain Diff
Merge branch 'improve_json' into phil
parents
9a7d98f8
b31c1856
Changes
59
Hide whitespace changes
Inline
Side-by-side
Showing
59 changed files
with
1934 additions
and
3691 deletions
+1934
-3691
qgroundcontrol.pro
qgroundcontrol.pro
+31
-21
GeoPoint3D.cpp
src/Wima/Geometry/GeoPoint3D.cpp
+0
-5
GeoPoint3D.h
src/Wima/Geometry/GeoPoint3D.h
+3
-9
Polygon2D.h
src/Wima/Geometry/Polygon2D.h
+8
-19
PolygonArray.h
src/Wima/Geometry/PolygonArray.h
+3
-6
WimaPolygonArray.h
src/Wima/Geometry/WimaPolygonArray.h
+7
-15
QNemoHeartbeat.h
src/Wima/Snake/QNemoHeartbeat.h
+2
-2
QNemoProgress.h
src/Wima/Snake/QNemoProgress.h
+4
-6
QtROSJsonFactory.h
src/Wima/Snake/QtROSJsonFactory.h
+0
-5
QtROSTypeFactory.h
src/Wima/Snake/QtROSTypeFactory.h
+0
-5
SnakeTilesLocal.h
src/Wima/Snake/SnakeTilesLocal.h
+1
-3
WimaController.cc
src/Wima/WimaController.cc
+126
-59
WimaController.h
src/Wima/WimaController.h
+6
-5
WimaController_new.cc
src/Wima/WimaController_new.cc
+0
-924
WimaController_new.h
src/Wima/WimaController_new.h
+0
-421
CasePacker.h
src/comm/ros_bridge/include/CasePacker.h
+0
-54
ComPrivateInclude.cpp
src/comm/ros_bridge/include/ComPrivateInclude.cpp
+0
-13
GenericMessages.h
src/comm/ros_bridge/include/GenericMessages.h
+0
-371
JsonFactory.h
src/comm/ros_bridge/include/JsonFactory.h
+0
-238
JsonMethodes.h
src/comm/ros_bridge/include/JsonMethodes.h
+0
-695
MessageBaseClass.h
src/comm/ros_bridge/include/MessageBaseClass.h
+0
-30
MessageGroups.h
src/comm/ros_bridge/include/MessageGroups.h
+0
-158
MessageTag.cpp
src/comm/ros_bridge/include/MessageTag.cpp
+0
-43
MessageTag.h
src/comm/ros_bridge/include/MessageTag.h
+0
-24
RosBridgeClient.cpp
src/comm/ros_bridge/include/RosBridgeClient.cpp
+77
-91
ThreadSafeQueue.h
src/comm/ros_bridge/include/ThreadSafeQueue.h
+0
-79
TopicPublisher.cpp
src/comm/ros_bridge/include/TopicPublisher.cpp
+0
-103
TypeFactory.h
src/comm/ros_bridge/include/TypeFactory.h
+0
-137
com_private.cpp
src/comm/ros_bridge/include/com_private.cpp
+53
-0
com_private.h
src/comm/ros_bridge/include/com_private.h
+8
-4
message_traits.h
src/comm/ros_bridge/include/message_traits.h
+15
-1
geopoint.cpp
.../ros_bridge/include/messages/geographic_msgs/geopoint.cpp
+5
-0
geopoint.h
...mm/ros_bridge/include/messages/geographic_msgs/geopoint.h
+149
-0
point32.cpp
...omm/ros_bridge/include/messages/geometry_msgs/point32.cpp
+6
-0
point32.h
src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
+139
-0
polygon.cpp
...omm/ros_bridge/include/messages/geometry_msgs/polygon.cpp
+6
-0
polygon.h
src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
+91
-0
polygon_stamped.cpp
...bridge/include/messages/geometry_msgs/polygon_stamped.cpp
+6
-0
polygon_stamped.h
...s_bridge/include/messages/geometry_msgs/polygon_stamped.h
+172
-0
polygon_array.cpp
...e/include/messages/jsk_recognition_msgs/polygon_array.cpp
+6
-0
polygon_array.h
...dge/include/messages/jsk_recognition_msgs/polygon_array.h
+328
-0
heartbeat.cpp
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp
+6
-0
heartbeat.h
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+53
-0
progress.cpp
src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp
+6
-0
progress.h
src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
+67
-0
header.cpp
src/comm/ros_bridge/include/messages/std_msgs/header.cpp
+69
-0
header.h
src/comm/ros_bridge/include/messages/std_msgs/header.h
+102
-0
time.cpp
src/comm/ros_bridge/include/messages/std_msgs/time.cpp
+5
-0
time.h
src/comm/ros_bridge/include/messages/std_msgs/time.h
+65
-0
ros_bridge.h
src/comm/ros_bridge/include/ros_bridge.h
+49
-0
server.cpp
src/comm/ros_bridge/include/server.cpp
+7
-9
server.h
src/comm/ros_bridge/include/server.h
+4
-7
topic_publisher.cpp
src/comm/ros_bridge/include/topic_publisher.cpp
+147
-0
topic_publisher.h
src/comm/ros_bridge/include/topic_publisher.h
+8
-23
topic_subscriber.cpp
src/comm/ros_bridge/include/topic_subscriber.cpp
+8
-15
topic_subscriber.h
src/comm/ros_bridge/include/topic_subscriber.h
+5
-11
CasePacker.cpp
src/comm/ros_bridge/src/CasePacker.cpp
+0
-69
ros_bridge.cpp
src/comm/ros_bridge/src/ros_bridge.cpp
+81
-0
utilities.h
src/comm/utilities.h
+0
-11
No files found.
qgroundcontrol.pro
View file @
32d51139
...
@@ -28,11 +28,11 @@ QGCROOT = $$PWD
...
@@ -28,11 +28,11 @@ QGCROOT = $$PWD
DebugBuild
{
DebugBuild
{
DESTDIR
=
$$
{
OUT_PWD
}
/
debug
DESTDIR
=
$$
{
OUT_PWD
}
/
debug
DEFINES
+=
DEBUG
DEFINES
+=
DEBUG
#
DEFINES
+=
ROS_BRIDGE_
CLIENT_
DEBUG
#
DEFINES
+=
ROS_BRIDGE_DEBUG
}
}
else
{
else
{
DESTDIR
=
$$
{
OUT_PWD
}
/
release
DESTDIR
=
$$
{
OUT_PWD
}
/
release
#
DEFINES
+=
DEBUG
#
DEFINES
+=
ROS_BRIDGE_
DEBUG
DEFINES
+=
NDEBUG
DEFINES
+=
NDEBUG
}
}
...
@@ -437,8 +437,7 @@ HEADERS += \
...
@@ -437,8 +437,7 @@ HEADERS += \
src
/
Wima
/
Geometry
/
PolygonArray
.
h
\
src
/
Wima
/
Geometry
/
PolygonArray
.
h
\
src
/
Wima
/
Snake
/
QNemoHeartbeat
.
h
\
src
/
Wima
/
Snake
/
QNemoHeartbeat
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
QtROSJsonFactory
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
QtROSTypeFactory
.
h
\
src
/
Wima
/
Snake
/
SnakeTiles
.
h
\
src
/
Wima
/
Snake
/
SnakeTiles
.
h
\
src
/
Wima
/
Snake
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
Snake
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
Snake
/
SnakeWorker
.
h
\
src
/
Wima
/
Snake
/
SnakeWorker
.
h
\
...
@@ -483,17 +482,21 @@ HEADERS += \
...
@@ -483,17 +482,21 @@ HEADERS += \
src
/
Wima
/
Geometry
/
testplanimetrycalculus
.
h
\
src
/
Wima
/
Geometry
/
testplanimetrycalculus
.
h
\
src
/
Settings
/
WimaSettings
.
h
\
src
/
Settings
/
WimaSettings
.
h
\
src
/
QmlControls
/
QmlObjectVectorModel
.
h
\
src
/
QmlControls
/
QmlObjectVectorModel
.
h
\
src
/
comm
/
ros_bridge
/
include
/
CasePacker
.
h
\
src
/
comm
/
ros_bridge
/
include
/
ComPrivateInclude
.
h
\
src
/
comm
/
ros_bridge
/
include
/
GenericMessages
.
h
\
src
/
comm
/
ros_bridge
/
include
/
JsonMethodes
.
h
\
src
/
comm
/
ros_bridge
/
include
/
MessageTag
.
h
\
src
/
comm
/
ros_bridge
/
include
/
MessageTraits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
RosBridgeClient
.
h
\
src
/
comm
/
ros_bridge
/
include
/
RosBridgeClient
.
h
\
src
/
comm
/
ros_bridge
/
include
/
Server
.
h
\
src
/
comm
/
ros_bridge
/
include
/
com_private
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TopicPublisher
.
h
\
src
/
comm
/
ros_bridge
/
include
/
message_traits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TopicSubscriber
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TypeFactory
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geometry_msgs
/
point32
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geometry_msgs
/
polygon
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geometry_msgs
/
polygon_stamped
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
jsk_recognition_msgs
/
polygon_array
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
h
\
src
/
comm
/
ros_bridge
/
include
/
server
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
time
.
h
\
src
/
comm
/
ros_bridge
/
include
/
topic_publisher
.
h
\
src
/
comm
/
ros_bridge
/
include
/
topic_subscriber
.
h
\
src
/
comm
/
utilities
.
h
src
/
comm
/
utilities
.
h
SOURCES
+=
\
SOURCES
+=
\
src
/
Snake
/
clipper
/
clipper
.
cpp
\
src
/
Snake
/
clipper
/
clipper
.
cpp
\
...
@@ -511,13 +514,20 @@ SOURCES += \
...
@@ -511,13 +514,20 @@ SOURCES += \
src
/
Wima
/
WaypointManager
/
Slicer
.
cpp
\
src
/
Wima
/
WaypointManager
/
Slicer
.
cpp
\
src
/
Wima
/
WaypointManager
/
Utils
.
cpp
\
src
/
Wima
/
WaypointManager
/
Utils
.
cpp
\
src
/
Wima
/
WimaBridge
.
cc
\
src
/
Wima
/
WimaBridge
.
cc
\
src
/
comm
/
ros_bridge
/
include
/
ComPrivateInclude
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
MessageTag
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
RosBridgeClient
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
RosBridgeClient
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
Server
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
com_private
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicPublisher
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicSubscriber
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geometry_msgs
/
point32
.
cpp
\
src
/
comm
/
ros_bridge
/
src
/
CasePacker
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geometry_msgs
/
polygon
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geometry_msgs
/
polygon_stamped
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
jsk_recognition_msgs
/
polygon_array
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
heartbeat
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
nemo_msgs
/
progress
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
header
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
std_msgs
/
time
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
server
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
topic_publisher
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
topic_subscriber
.
cpp
\
src
/
Wima
/
Snake
/
snaketile
.
cpp
\
src
/
Wima
/
Snake
/
snaketile
.
cpp
\
src
/
api
/
QGCCorePlugin
.
cc
\
src
/
api
/
QGCCorePlugin
.
cc
\
src
/
api
/
QGCOptions
.
cc
\
src
/
api
/
QGCOptions
.
cc
\
...
@@ -549,7 +559,7 @@ SOURCES += \
...
@@ -549,7 +559,7 @@ SOURCES += \
src
/
Wima
/
Geometry
/
testplanimetrycalculus
.
cpp
\
src
/
Wima
/
Geometry
/
testplanimetrycalculus
.
cpp
\
src
/
Settings
/
WimaSettings
.
cc
\
src
/
Settings
/
WimaSettings
.
cc
\
src
/
QmlControls
/
QmlObjectVectorModel
.
cc
\
src
/
QmlControls
/
QmlObjectVectorModel
.
cc
\
src
/
comm
/
ros_bridge
/
src
/
ROSB
ridge
.
cpp
src
/
comm
/
ros_bridge
/
src
/
ros_b
ridge
.
cpp
#
#
#
Unit
Test
specific
configuration
goes
here
(
requires
full
debug
build
with
all
plugins
)
#
Unit
Test
specific
configuration
goes
here
(
requires
full
debug
build
with
all
plugins
)
...
...
src/Wima/Geometry/GeoPoint3D.cpp
View file @
32d51139
...
@@ -19,11 +19,6 @@ GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
...
@@ -19,11 +19,6 @@ GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
:
QObject
(
parent
),
ROSGeoPoint
(
p
.
latitude
(),
p
.
longitude
(),
p
.
altitude
())
:
QObject
(
parent
),
ROSGeoPoint
(
p
.
latitude
(),
p
.
longitude
(),
p
.
altitude
())
{}
{}
GeoPoint3D
*
GeoPoint3D
::
Clone
()
const
{
return
new
GeoPoint3D
(
*
this
);
}
GeoPoint3D
&
GeoPoint3D
::
operator
=
(
const
GeoPoint3D
&
p
)
GeoPoint3D
&
GeoPoint3D
::
operator
=
(
const
GeoPoint3D
&
p
)
{
{
this
->
setLatitude
(
p
.
latitude
());
this
->
setLatitude
(
p
.
latitude
());
...
...
src/Wima/Geometry/GeoPoint3D.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/JsonMethodes.h"
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/GenericMessages.h"
#include <QGeoCoordinate>
#include <QGeoCoordinate>
#include <QObject>
#include <QObject>
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
typedef
ROSBridge
::
MessageBaseClass
ROSMsg
;
typedef
ros_bridge
::
messages
::
geographic_msgs
::
geo_point
::
GeoPoint
ROSGeoPoint
;
typedef
ROSBridge
::
GenericMessages
::
GeographicMsgs
::
GeoPoint
ROSGeoPoint
;
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
class
GeoPoint3D
:
public
QObject
,
public
ROSGeoPoint
class
GeoPoint3D
:
public
QObject
,
public
ROSGeoPoint
{
{
Q_OBJECT
Q_OBJECT
public:
public:
typedef
MsgGroups
::
GeoPointGroup
Group
;
GeoPoint3D
(
QObject
*
parent
=
nullptr
);
GeoPoint3D
(
QObject
*
parent
=
nullptr
);
GeoPoint3D
(
double
latitude
,
GeoPoint3D
(
double
latitude
,
...
@@ -29,7 +24,6 @@ public:
...
@@ -29,7 +24,6 @@ public:
GeoPoint3D
(
const
QGeoCoordinate
&
p
,
GeoPoint3D
(
const
QGeoCoordinate
&
p
,
QObject
*
parent
=
nullptr
);
QObject
*
parent
=
nullptr
);
virtual
GeoPoint3D
*
Clone
()
const
override
;
GeoPoint3D
&
operator
=
(
const
GeoPoint3D
&
p
);
GeoPoint3D
&
operator
=
(
const
GeoPoint3D
&
p
);
GeoPoint3D
&
operator
=
(
const
QGeoCoordinate
&
p
);
GeoPoint3D
&
operator
=
(
const
QGeoCoordinate
&
p
);
...
...
src/Wima/Geometry/Polygon2D.h
View file @
32d51139
...
@@ -3,41 +3,30 @@
...
@@ -3,41 +3,30 @@
#include <QPolygonF>
#include <QPolygonF>
#include <QPointF>
#include <QPointF>
#include "ros_bridge/include/MessageGroups.h"
#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
#include "ros_bridge/include/GenericMessages.h"
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/JsonMethodes.h"
namespace
MsgGroupsNS
=
ROSBridge
::
MessageGroups
;
namespace
polygon_stamped
=
ros_bridge
::
messages
::
geometry_msgs
::
polygon_stamped
;
namespace
PolyStampedNS
=
ROSBridge
::
JsonMethodes
::
GeometryMsgs
::
PolygonStamped
;
typedef
ROSBridge
::
MessageBaseClass
ROSMsg
;
template
<
class
PointType
=
QPointF
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
>
template
<
class
PointType
=
QPointF
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
>
class
Polygon2DTemplate
:
public
ROSMsg
{
//
class
Polygon2DTemplate
{
//
typedef
ROSBridge
::
GenericMessages
::
GeometryMsgs
::
GenericPolygon
<
PointType
,
ContainerType
>
Poly
;
typedef
ros_bridge
::
messages
::
geometry_msgs
::
polygon
::
GenericPolygon
<
PointType
,
ContainerType
>
Polygon
;
public:
public:
typedef
MsgGroupsNS
::
PolygonStampedGroup
Group
;
// has no header
Polygon2DTemplate
(){}
Polygon2DTemplate
(){}
Polygon2DTemplate
(
const
Polygon2DTemplate
&
other
)
:
ROSMsg
(),
_polygon
(
other
.
_polygon
){}
Polygon2DTemplate
(
const
Polygon2DTemplate
&
other
)
:
_polygon
(
other
.
_polygon
){}
Polygon2DTemplate
&
operator
=
(
const
Polygon2DTemplate
&
other
)
{
Polygon2DTemplate
&
operator
=
(
const
Polygon2DTemplate
&
other
)
{
this
->
_polygon
=
other
.
_polygon
;
this
->
_polygon
=
other
.
_polygon
;
return
*
this
;
return
*
this
;
}
}
const
Poly
&
polygon
()
const
{
return
_polygon
;}
const
Poly
gon
&
polygon
()
const
{
return
_polygon
;}
Poly
&
polygon
()
{
return
_polygon
;}
Poly
gon
&
polygon
()
{
return
_polygon
;}
const
ContainerType
<
PointType
>
&
path
()
const
{
return
_polygon
.
points
();}
const
ContainerType
<
PointType
>
&
path
()
const
{
return
_polygon
.
points
();}
ContainerType
<
PointType
>
&
path
()
{
return
_polygon
.
points
();}
ContainerType
<
PointType
>
&
path
()
{
return
_polygon
.
points
();}
virtual
Polygon2DTemplate
*
Clone
()
const
{
// ROSMsg
return
new
Polygon2DTemplate
(
*
this
);
}
private:
private:
Poly
_polygon
;
Poly
gon
_polygon
;
};
};
...
...
src/Wima/Geometry/PolygonArray.h
View file @
32d51139
#pragma once
#pragma once
#include <Q
Object
>
#include <Q
String
>
#include "ros_bridge/include/MessageBaseClass.h"
typedef
ROSBridge
::
MessageBaseClass
ROSMsgBase
;
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
>
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
>
class
PolygonArray
:
public
ROSMsgBase
,
public
ContainerType
<
PolygonType
>
{
class
PolygonArray
:
public
ContainerType
<
PolygonType
>
{
public:
public:
explicit
PolygonArray
()
:
ROSMsgBase
(),
ContainerType
<
PolygonType
>
()
{}
explicit
PolygonArray
()
:
ContainerType
<
PolygonType
>
()
{}
PolygonArray
(
const
PolygonArray
&
other
)
:
ContainerType
<
PolygonType
>
(
other
)
{}
PolygonArray
(
const
PolygonArray
&
other
)
:
ContainerType
<
PolygonType
>
(
other
)
{}
QString
type
()
const
override
{
return
"PolygonArray"
;}
QString
type
()
const
override
{
return
"PolygonArray"
;}
...
...
src/Wima/Geometry/WimaPolygonArray.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/MessageGroups.h"
#include "QmlObjectListModel.h"
#include "QmlObjectListModel.h"
#include <QVector>
#include <QVector>
#include <QString>
#include <QString>
typedef
ROSBridge
::
MessageBaseClass
ROSMsg
;
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
>
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
class
WimaPolygonArray
typedef
MsgGroups
::
EmptyGroup
EmptyGroup
;
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
,
typename
GroupType
=
EmptyGroup
>
class
WimaPolygonArray
:
public
ROSMsg
{
{
public:
public:
typedef
GroupType
Group
;
WimaPolygonArray
()
{}
WimaPolygonArray
()
{}
WimaPolygonArray
(
const
WimaPolygonArray
&
other
)
:
WimaPolygonArray
(
const
WimaPolygonArray
&
other
)
:
ROSMsg
()
_polygons
(
other
.
_polygons
),
_dirty
(
true
)
,
_polygons
(
other
.
_polygons
),
_dirty
(
true
)
{}
{}
~
WimaPolygonArray
(){
virtual
WimaPolygonArray
*
Clone
()
const
override
{
_objs
.
clearAndDeleteContents
();
return
new
WimaPolygonArray
(
*
this
);
}
}
class
QmlObjectListModel
*
QmlObjectListModel
(){
class
QmlObjectListModel
*
QmlObjectListModel
(){
...
@@ -45,9 +37,9 @@ public:
...
@@ -45,9 +37,9 @@ public:
private:
private:
void
_updateObjects
(
void
){
void
_updateObjects
(
void
){
_objs
.
clear
();
_objs
.
clear
AndDeleteContents
();
for
(
long
i
=
0
;
i
<
_polygons
.
size
();
++
i
){
for
(
long
i
=
0
;
i
<
_polygons
.
size
();
++
i
){
_objs
.
append
(
&
_polygons
[
i
]
);
_objs
.
append
(
new
PolygonType
(
_polygons
[
i
])
);
}
}
}
}
...
...
src/Wima/Snake/QNemoHeartbeat.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/
GenericMessages
.h"
#include "ros_bridge/include/
messages/nemo_msgs/heartbeat
.h"
using
QNemoHeartbeat
=
ROSBridge
::
GenericMessages
::
NemoMsgs
::
Heartbeat
;
using
QNemoHeartbeat
=
ros_bridge
::
messages
::
nemo_msgs
::
heartbeat
::
Heartbeat
;
src/Wima/Snake/QNemoProgress.h
View file @
32d51139
#pragma once
#pragma once
#include <QVector>
#include <QObject>
#include "ros_bridge/include/GenericMessages.h"
namespace
NemoMsgs
=
ROSBridge
::
GenericMessages
::
NemoMsgs
;
#include <QVector>
typedef
NemoMsgs
::
GenericProgress
<
int
,
QVector
>
QNemoProgress
;
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
namespace
nemo
=
ros_bridge
::
messages
::
nemo_msgs
;
typedef
nemo
::
progress
::
GenericProgress
<
int
,
QVector
>
QNemoProgress
;
src/Wima/Snake/QtROSJsonFactory.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#include "ros_bridge/include/JsonFactory.h"
#include <QString>
typedef
ROSBridge
::
GenericJsonFactory
<>
QtROSJsonFactory
;
src/Wima/Snake/QtROSTypeFactory.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#include "ros_bridge/include/TypeFactory.h"
#include <QString>
typedef
ROSBridge
::
TypeFactory
QtROSTypeFactory
;
src/Wima/Snake/SnakeTilesLocal.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/MessageGroups.h"
#include "Wima/Geometry/Polygon2D.h"
#include "Wima/Geometry/Polygon2D.h"
#include "Wima/Geometry/WimaPolygonArray.h"
#include "Wima/Geometry/WimaPolygonArray.h"
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
typedef
WimaPolygonArray
<
Polygon2D
,
QVector
>
SnakeTilesLocal
;
typedef
WimaPolygonArray
<
Polygon2D
,
QVector
,
MsgGroups
::
PolygonArrayGroup
>
SnakeTilesLocal
;
src/Wima/WimaController.cc
View file @
32d51139
#include "WimaController.h"
#include "WimaController.h"
#include "utilities.h"
#include "utilities.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/include/CasePacker.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "Snake/QtROSJsonFactory.h"
#include "Snake/QtROSTypeFactory.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoHeartbeat.h"
...
@@ -87,6 +89,7 @@ WimaController::WimaController(QObject *parent)
...
@@ -87,6 +89,7 @@ WimaController::WimaController(QObject *parent)
,
_batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
1000
/*ms*/
)
,
_batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
1000
/*ms*/
)
,
_snakeTicker
(
EVENT_TIMER_INTERVAL
,
200
/*ms*/
)
,
_snakeTicker
(
EVENT_TIMER_INTERVAL
,
200
/*ms*/
)
,
_nemoTimeoutTicker
(
EVENT_TIMER_INTERVAL
,
5000
/*ms*/
)
,
_nemoTimeoutTicker
(
EVENT_TIMER_INTERVAL
,
5000
/*ms*/
)
,
_topicServiceSetupDone
(
false
)
{
{
// ROS Bridge.
// ROS Bridge.
...
@@ -94,11 +97,11 @@ WimaController::WimaController(QObject *parent)
...
@@ -94,11 +97,11 @@ WimaController::WimaController(QObject *parent)
auto
connectionStringFact
=
wimaSettings
->
rosbridgeConnectionString
();
auto
connectionStringFact
=
wimaSettings
->
rosbridgeConnectionString
();
auto
setConnectionString
=
[
connectionStringFact
,
this
]{
auto
setConnectionString
=
[
connectionStringFact
,
this
]{
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
if
(
ROSB
ridge
::
isValidConnectionString
(
connectionString
.
toLocal8Bit
().
data
())
){
if
(
ros_b
ridge
::
isValidConnectionString
(
connectionString
.
toLocal8Bit
().
data
())
){
this
->
_pRosBridge
.
reset
(
new
ROSB
ridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
this
->
_pRosBridge
.
reset
(
new
ros_b
ridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
}
else
{
}
else
{
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
connectionString
);
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
connectionString
);
this
->
_pRosBridge
.
reset
(
new
ROSB
ridge
::
ROSBridge
(
"localhost:9090"
));
this
->
_pRosBridge
.
reset
(
new
ros_b
ridge
::
ROSBridge
(
"localhost:9090"
));
}
}
};
};
setConnectionString
();
setConnectionString
();
...
@@ -549,14 +552,29 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
...
@@ -549,14 +552,29 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
emit
snakeTileCenterPointsChanged
();
emit
snakeTileCenterPointsChanged
();
if
(
_enableSnake
.
rawValue
().
toBool
()
if
(
_enableSnake
.
rawValue
().
toBool
()
&&
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
){
&&
_pRosBridge
->
connected
()
if
(
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
&&
_topicServiceSetupDone
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
&&
_snakeTilesLocal
.
polygons
().
size
()
>
0
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
)
}
{
using
namespace
ros_bridge
::
messages
;
// Publish snake origin.
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
_snakeOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
_pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
_snakeTilesLocal
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
_pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
}
}
_localPlanDataValid
=
true
;
_localPlanDataValid
=
true
;
return
true
;
return
true
;
}
}
...
@@ -747,21 +765,24 @@ void WimaController::_eventTimerHandler()
...
@@ -747,21 +765,24 @@ void WimaController::_eventTimerHandler()
}
}
if
(
_snakeTicker
.
ready
()
)
{
if
(
_snakeTicker
.
ready
()
)
{
static
bool
setupDone
=
false
;
if
(
_enableSnake
.
rawValue
().
toBool
()
)
{
if
(
_enableSnake
.
rawValue
().
toBool
()
)
{
if
(
!
_pRosBridge
->
isRunning
())
{
if
(
!
_pRosBridge
->
isRunning
())
{
_pRosBridge
->
start
();
_pRosBridge
->
start
();
}
else
if
(
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
&&
!
setupDone
)
{
}
else
if
(
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
&&
!
_topicServiceSetupDone
)
{
if
(
_doTopicServiceSetup
()
)
_topicServiceSetupDone
=
true
;
}
else
if
(
_pRosBridge
->
isRunning
()
&&
!
_pRosBridge
->
connected
()
&&
_topicServiceSetupDone
){
_pRosBridge
->
reset
();
_pRosBridge
->
reset
();
_pRosBridge
->
start
();
_pRosBridge
->
start
();
_setupTopicService
();
_topicServiceSetupDone
=
false
;
setupDone
=
true
;
}
else
if
(
_pRosBridge
->
isRunning
()
&&
!
_pRosBridge
->
connected
()
){
setupDone
=
false
;
}
}
}
else
if
(
_pRosBridge
->
isRunning
()
)
{
}
else
if
(
_pRosBridge
->
isRunning
()
)
{
_pRosBridge
->
reset
();
_pRosBridge
->
reset
();
s
etupDone
=
false
;
_topicServiceS
etupDone
=
false
;
}
}
}
}
}
}
...
@@ -937,35 +958,74 @@ void WimaController::_switchSnakeManager(QVariant variant)
...
@@ -937,35 +958,74 @@ void WimaController::_switchSnakeManager(QVariant variant)
}
}
}
}
void
WimaController
::
_setupTopicService
()
bool
WimaController
::
_doTopicServiceSetup
()
{
{
if
(
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
using
namespace
ros_bridge
::
messages
;
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
if
(
_snakeTilesLocal
.
polygons
().
size
()
==
0
)
}
return
false
;
// Publish snake origin.
_pRosBridge
->
advertiseTopic
(
"/snake/origin"
,
geographic_msgs
::
geo_point
::
messageType
().
c_str
());
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
_snakeOrigin
,
*
jOrigin
.
get
(),
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
_pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
_pRosBridge
->
advertiseTopic
(
"/snake/tiles"
,
jsk_recognition_msgs
::
polygon_array
::
messageType
().
c_str
());
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
_snakeTilesLocal
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
_pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
// Subscribe nemo progress.
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
int
requiredSize
=
this
->
_snakeTilesLocal
.
polygons
().
size
();
int
requiredSize
=
this
->
_snakeTilesLocal
.
polygons
().
size
();
auto
&
progress
=
this
->
_nemoProgress
;
auto
&
progress
_msg
=
this
->
_nemoProgress
;
if
(
!
this
->
_pRosBridge
->
casePacker
()
->
unpack
(
pDoc
,
progress
)
if
(
!
nemo_msgs
::
progress
::
fromJson
(
*
pDoc
,
progress_msg
)
||
progress
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
||
progress
_msg
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
// Set progress to default.
// Set progress to default.
progress
.
progress
().
fill
(
0
,
requiredSize
);
progress_msg
.
progress
().
fill
(
0
,
requiredSize
);
// Publish origin and tiles again.
if
(
this
->
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
// Publish snake origin.
this
->
_pRosBridge
->
publish
(
this
->
_snakeOrigin
,
"/snake/origin"
);
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
this
->
_pRosBridge
->
publish
(
this
->
_snakeTilesLocal
,
"/snake/tiles"
);
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
}
this
->
_snakeOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
_pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
_snakeTilesLocal
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
_pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
}
}
emit
WimaController
::
nemoProgressChanged
();
emit
WimaController
::
nemoProgressChanged
();
});
});
// Subscribe /nemo/heartbeat.
_pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
_pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
if
(
!
this
->
_pRosBridge
->
casePacker
()
->
unpack
(
pDoc
,
this
->
_nemoHeartbeat
)
)
{
auto
&
heartbeat_msg
=
this
->
_nemoHeartbeat
;
if
(
this
->
_nemoHeartbeat
.
status
()
==
this
->
_fallbackStatus
)
if
(
!
nemo_msgs
::
heartbeat
::
fromJson
(
*
pDoc
,
heartbeat_msg
)
)
{
if
(
heartbeat_msg
.
status
()
==
this
->
_fallbackStatus
)
return
;
return
;
this
->
_nemoHeartbeat
.
setStatus
(
this
->
_fallbackStatus
);
heartbeat_msg
.
setStatus
(
this
->
_fallbackStatus
);
}
}
this
->
_nemoTimeoutTicker
.
reset
();
this
->
_nemoTimeoutTicker
.
reset
();
...
@@ -974,31 +1034,38 @@ void WimaController::_setupTopicService()
...
@@ -974,31 +1034,38 @@ void WimaController::_setupTopicService()
emit
WimaController
::
nemoStatusStringChanged
();
emit
WimaController
::
nemoStatusStringChanged
();
});
});
_pRosBridge
->
advertiseService
(
"/snake/get_origin"
,
"snake_msgs/GetOrigin"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
JsonDocUPtr
pDoc
;
if
(
this
->
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
pDoc
=
this
->
_pRosBridge
->
casePacker
()
->
pack
(
this
->
_snakeOrigin
,
""
);
}
else
{
pDoc
=
this
->
_pRosBridge
->
casePacker
()
->
pack
(
::
GeoPoint3D
(
0
,
0
,
0
),
""
);
}
this
->
_pRosBridge
->
casePacker
()
->
removeTag
(
pDoc
);
rapidjson
::
Document
value
(
rapidjson
::
kObjectType
);
JsonDocUPtr
pReturn
(
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
));
value
.
CopyFrom
(
*
pDoc
,
pReturn
->
GetAllocator
());
pReturn
->
AddMember
(
"origin"
,
value
,
pReturn
->
GetAllocator
());
return
pReturn
;
// Advertise /snake/get_origin.
_pRosBridge
->
advertiseService
(
"/snake/get_origin"
,
"snake_msgs/GetOrigin"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
using
namespace
ros_bridge
::
messages
;
JsonDocUPtr
pDoc
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
::
GeoPoint3D
origin
=
this
->
_snakeOrigin
;
rapidjson
::
Value
jOrigin
(
rapidjson
::
kObjectType
);
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
origin
,
jOrigin
,
pDoc
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
return
pDoc
;
});
});
// Advertise /snake/get_tiles.
_pRosBridge
->
advertiseService
(
"/snake/get_tiles"
,
"snake_msgs/GetTiles"
,
_pRosBridge
->
advertiseService
(
"/snake/get_tiles"
,
"snake_msgs/GetTiles"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
JsonDocUPtr
pDoc
=
this
->
_pRosBridge
->
casePacker
()
->
pack
(
this
->
_snakeTilesLocal
,
""
);
{
this
->
_pRosBridge
->
casePacker
()
->
removeTag
(
pDoc
);
JsonDocUPtr
pDoc
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
rapidjson
::
Document
value
(
rapidjson
::
kObjectType
);
rapidjson
::
Value
jSnakeTiles
(
rapidjson
::
kObjectType
);
JsonDocUPtr
pReturn
(
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
));
bool
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
value
.
CopyFrom
(
*
pDoc
,
pReturn
->
GetAllocator
());
this
->
_snakeTilesLocal
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
pReturn
->
AddMember
(
"tiles"
,
value
,
pReturn
->
GetAllocator
());
Q_ASSERT
(
ret
);
return
pReturn
;
(
void
)
ret
;
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
return
pDoc
;
});
});
return
true
;
}
}
src/Wima/WimaController.h
View file @
32d51139
...
@@ -35,12 +35,13 @@
...
@@ -35,12 +35,13 @@
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoHeartbeat.h"
#include "ros_bridge/include/ROSBridge.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/include/CasePacker.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"
#include "WaypointManager/RTLManager.h"
#include "utilities.h"
#include <map>
#include <map>
...
@@ -54,7 +55,7 @@ class WimaController : public QObject
...
@@ -54,7 +55,7 @@ class WimaController : public QObject
enum
FileType
{
WimaFile
,
PlanFile
};
enum
FileType
{
WimaFile
,
PlanFile
};
typedef
QScopedPointer
<
ROSB
ridge
::
ROSBridge
>
ROSBridgePtr
;
typedef
QScopedPointer
<
ros_b
ridge
::
ROSBridge
>
ROSBridgePtr
;
public:
public:
...
@@ -336,7 +337,7 @@ private slots:
...
@@ -336,7 +337,7 @@ private slots:
void
_snakeStoreWorkerResults
();
void
_snakeStoreWorkerResults
();
void
_initStartSnakeWorker
();
void
_initStartSnakeWorker
();
void
_switchSnakeManager
(
QVariant
variant
);
void
_switchSnakeManager
(
QVariant
variant
);
void
_setupTopicService
();
bool
_doTopicServiceSetup
();
// Periodic tasks.
// Periodic tasks.
void
_eventTimerHandler
(
void
);
void
_eventTimerHandler
(
void
);
// Waypoint manager handling.
// Waypoint manager handling.
...
@@ -344,7 +345,6 @@ private slots:
...
@@ -344,7 +345,6 @@ private slots:
private:
private:
using
StatusMap
=
std
::
map
<
int
,
QString
>
;
using
StatusMap
=
std
::
map
<
int
,
QString
>
;
using
CasePacker
=
ROSBridge
::
CasePacker
;
// Controllers.
// Controllers.
PlanMasterController
*
_masterController
;
PlanMasterController
*
_masterController
;
...
@@ -407,6 +407,7 @@ private:
...
@@ -407,6 +407,7 @@ private:
int
_fallbackStatus
;
int
_fallbackStatus
;
ROSBridgePtr
_pRosBridge
;
ROSBridgePtr
_pRosBridge
;
static
StatusMap
_nemoStatusMap
;
static
StatusMap
_nemoStatusMap
;
bool
_topicServiceSetupDone
;
// Periodic tasks.
// Periodic tasks.
QTimer
_eventTimer
;
QTimer
_eventTimer
;
...
...
src/Wima/WimaController_new.cc
deleted
100644 → 0
View file @
9a7d98f8
#include "WimaController.h"
#include "utilities.h"
#include "ros_bridge/include/JsonMethodes.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 "ros_bridge/include/CasePacker.h"
#include "Snake/QtROSJsonFactory.h"
#include "Snake/QtROSTypeFactory.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "QVector3D"
#include <QScopedPointer>
// const char* WimaController::wimaFileExtension = "wima";
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
const
char
*
WimaController
::
missionItemsName
=
"MissionItems"
;
const
char
*
WimaController
::
settingsGroup
=
"WimaController"
;
const
char
*
WimaController
::
enableWimaControllerName
=
"EnableWimaController"
;
const
char
*
WimaController
::
overlapWaypointsName
=
"OverlapWaypoints"
;
const
char
*
WimaController
::
maxWaypointsPerPhaseName
=
"MaxWaypointsPerPhase"
;
const
char
*
WimaController
::
startWaypointIndexName
=
"StartWaypointIndex"
;
const
char
*
WimaController
::
showAllMissionItemsName
=
"ShowAllMissionItems"
;
const
char
*
WimaController
::
showCurrentMissionItemsName
=
"ShowCurrentMissionItems"
;
const
char
*
WimaController
::
flightSpeedName
=
"FlightSpeed"
;
const
char
*
WimaController
::
arrivalReturnSpeedName
=
"ArrivalReturnSpeed"
;
const
char
*
WimaController
::
altitudeName
=
"Altitude"
;
const
char
*
WimaController
::
snakeTileWidthName
=
"SnakeTileWidth"
;
const
char
*
WimaController
::
snakeTileHeightName
=
"SnakeTileHeight"
;
const
char
*
WimaController
::
snakeMinTileAreaName
=
"SnakeMinTileArea"
;
const
char
*
WimaController
::
snakeLineDistanceName
=
"SnakeLineDistance"
;
const
char
*
WimaController
::
snakeMinTransectLengthName
=
"SnakeMinTransectLength"
;
WimaController
::
StatusMap
WimaController
::
_nemoStatusMap
{
std
::
make_pair
<
int
,
QString
>
(
0
,
"No Heartbeat"
),
std
::
make_pair
<
int
,
QString
>
(
1
,
"Connected"
),
std
::
make_pair
<
int
,
QString
>
(
-
1
,
"Timeout"
)};
using
namespace
snake
;
using
namespace
snake_geometry
;
WimaController
::
WimaController
(
QObject
*
parent
)
:
QObject
(
parent
)
,
_joinedArea
()
,
_measurementArea
()
,
_serviceArea
()
,
_corridor
()
,
_localPlanDataValid
(
false
)
,
_areaInterface
(
&
_measurementArea
,
&
_serviceArea
,
&
_corridor
,
&
_joinedArea
)
,
_managerSettings
()
,
_defaultManager
(
_managerSettings
,
_areaInterface
)
,
_snakeManager
(
_managerSettings
,
_areaInterface
)
,
_rtlManager
(
_managerSettings
,
_areaInterface
)
,
_currentManager
(
&
_defaultManager
)
,
_managerList
{
&
_defaultManager
,
&
_snakeManager
,
&
_rtlManager
}
,
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/WimaController.SettingsGroup.json"
),
this
))
,
_enableWimaController
(
settingsGroup
,
_metaDataMap
[
enableWimaControllerName
])
,
_overlapWaypoints
(
settingsGroup
,
_metaDataMap
[
overlapWaypointsName
])
,
_maxWaypointsPerPhase
(
settingsGroup
,
_metaDataMap
[
maxWaypointsPerPhaseName
])
,
_nextPhaseStartWaypointIndex
(
settingsGroup
,
_metaDataMap
[
startWaypointIndexName
])
,
_showAllMissionItems
(
settingsGroup
,
_metaDataMap
[
showAllMissionItemsName
])
,
_showCurrentMissionItems
(
settingsGroup
,
_metaDataMap
[
showCurrentMissionItemsName
])
,
_flightSpeed
(
settingsGroup
,
_metaDataMap
[
flightSpeedName
])
,
_arrivalReturnSpeed
(
settingsGroup
,
_metaDataMap
[
arrivalReturnSpeedName
])
,
_altitude
(
settingsGroup
,
_metaDataMap
[
altitudeName
])
,
_measurementPathLength
(
-
1
)
,
_lowBatteryHandlingTriggered
(
false
)
,
_snakeCalcInProgress
(
false
)
,
_snakeTileWidth
(
settingsGroup
,
_metaDataMap
[
snakeTileWidthName
])
,
_snakeTileHeight
(
settingsGroup
,
_metaDataMap
[
snakeTileHeightName
])
,
_snakeMinTileArea
(
settingsGroup
,
_metaDataMap
[
snakeMinTileAreaName
])
,
_snakeLineDistance
(
settingsGroup
,
_metaDataMap
[
snakeLineDistanceName
])
,
_snakeMinTransectLength
(
settingsGroup
,
_metaDataMap
[
snakeMinTransectLengthName
])
,
_nemoHeartbeat
(
0
/*status: not connected*/
)
,
_fallbackStatus
(
0
/*status: not connected*/
)
,
_bridgeSetupDone
(
false
)
,
_pRosBridge
(
new
ROSBridge
::
ROSBridge
())
{
// Set up facts.
_showAllMissionItems
.
setRawValue
(
true
);
_showCurrentMissionItems
.
setRawValue
(
true
);
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateOverlap
);
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
connect
(
&
_flightSpeed
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateflightSpeed
);
connect
(
&
_arrivalReturnSpeed
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateArrivalReturnSpeed
);
connect
(
&
_altitude
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateAltitude
);
// Init waypoint managers.
bool
value
;
size_t
overlap
=
_overlapWaypoints
.
rawValue
().
toUInt
(
&
value
);
Q_ASSERT
(
value
);
size_t
N
=
_maxWaypointsPerPhase
.
rawValue
().
toUInt
(
&
value
);
Q_ASSERT
(
value
);
size_t
startIdx
=
_nextPhaseStartWaypointIndex
.
rawValue
().
toUInt
(
&
value
);
Q_ASSERT
(
value
);
(
void
)
value
;
for
(
auto
manager
:
_managerList
){
manager
->
setOverlap
(
overlap
);
manager
->
setN
(
N
);
manager
->
setStartIndex
(
startIdx
);
}
// Periodic.
connect
(
&
_eventTimer
,
&
QTimer
::
timeout
,
this
,
&
WimaController
::
_eventTimerHandler
);
//_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
_eventTimer
.
start
(
EVENT_TIMER_INTERVAL
);
// Snake Worker Thread.
connect
(
&
_snakeWorker
,
&
SnakeDataManager
::
finished
,
this
,
&
WimaController
::
_updateSnakeData
);
connect
(
this
,
&
WimaController
::
nemoProgressChanged
,
this
,
&
WimaController
::
_initStartSnakeWorker
);
connect
(
this
,
&
QObject
::
destroyed
,
&
this
->
_snakeWorker
,
&
SnakeDataManager
::
quit
);
// Snake.
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_switchSnakeManager
);
_switchSnakeManager
(
_enableSnake
.
rawValue
());
}
PlanMasterController
*
WimaController
::
masterController
()
{
return
_masterController
;
}
MissionController
*
WimaController
::
missionController
()
{
return
_missionController
;
}
QmlObjectListModel
*
WimaController
::
visualItems
()
{
return
&
_areas
;
}
QmlObjectListModel
*
WimaController
::
missionItems
()
{
return
const_cast
<
QmlObjectListModel
*>
(
&
_currentManager
->
missionItems
());
}
QmlObjectListModel
*
WimaController
::
currentMissionItems
()
{
return
const_cast
<
QmlObjectListModel
*>
(
&
_currentManager
->
currentMissionItems
());
}
QVariantList
WimaController
::
waypointPath
()
{
return
const_cast
<
QVariantList
&>
(
_currentManager
->
waypointsVariant
());
}
QVariantList
WimaController
::
currentWaypointPath
()
{
return
const_cast
<
QVariantList
&>
(
_currentManager
->
currentWaypointsVariant
());
}
Fact
*
WimaController
::
enableWimaController
()
{
return
&
_enableWimaController
;
}
Fact
*
WimaController
::
overlapWaypoints
()
{
return
&
_overlapWaypoints
;
}
Fact
*
WimaController
::
maxWaypointsPerPhase
()
{
return
&
_maxWaypointsPerPhase
;
}
Fact
*
WimaController
::
startWaypointIndex
()
{
return
&
_nextPhaseStartWaypointIndex
;
}
Fact
*
WimaController
::
showAllMissionItems
()
{
return
&
_showAllMissionItems
;
}
Fact
*
WimaController
::
showCurrentMissionItems
()
{
return
&
_showCurrentMissionItems
;
}
Fact
*
WimaController
::
flightSpeed
()
{
return
&
_flightSpeed
;
}
Fact
*
WimaController
::
arrivalReturnSpeed
()
{
return
&
_arrivalReturnSpeed
;
}
Fact
*
WimaController
::
altitude
()
{
return
&
_altitude
;
}
QVector
<
int
>
WimaController
::
nemoProgress
()
{
if
(
_nemoProgress
.
progress
().
size
()
==
_snakeTileCenterPoints
.
size
()
)
return
_nemoProgress
.
progress
();
else
return
QVector
<
int
>
(
_snakeTileCenterPoints
.
size
(),
0
);
}
double
WimaController
::
phaseDistance
()
const
{
return
0.0
;
}
double
WimaController
::
phaseDuration
()
const
{
return
0.0
;
}
int
WimaController
::
nemoStatus
()
const
{
return
_nemoHeartbeat
.
status
();
}
QString
WimaController
::
nemoStatusString
()
const
{
return
_nemoStatusMap
.
at
(
_nemoHeartbeat
.
status
());
}
bool
WimaController
::
snakeCalcInProgress
()
const
{
return
_snakeCalcInProgress
;
}
void
WimaController
::
setMasterController
(
PlanMasterController
*
masterC
)
{
_masterController
=
masterC
;
_managerSettings
.
setMasterController
(
masterC
);
emit
masterControllerChanged
();
}
void
WimaController
::
setMissionController
(
MissionController
*
missionC
)
{
_missionController
=
missionC
;
_managerSettings
.
setMissionController
(
missionC
);
emit
missionControllerChanged
();
}
void
WimaController
::
nextPhase
()
{
_calcNextPhase
();
}
void
WimaController
::
previousPhase
()
{
if
(
!
_currentManager
->
previous
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
resetPhase
()
{
if
(
!
_currentManager
->
reset
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
requestSmartRTL
()
{
QString
errorString
(
"Smart RTL requested. "
);
if
(
!
_checkSmartRTLPreCondition
(
errorString
)
){
qgcApp
()
->
showMessage
(
errorString
);
return
;
}
emit
smartRTLRequestConfirm
();
}
bool
WimaController
::
upload
()
{
auto
&
currentMissionItems
=
_defaultManager
.
currentMissionItems
();
if
(
!
_serviceArea
.
containsCoordinate
(
_masterController
->
managerVehicle
()
->
coordinate
())
&&
currentMissionItems
.
count
()
>
0
)
{
emit
forceUploadConfirm
();
return
false
;
}
return
forceUpload
();
}
bool
WimaController
::
forceUpload
()
{
auto
&
currentMissionItems
=
_defaultManager
.
currentMissionItems
();
if
(
currentMissionItems
.
count
()
<
1
)
return
false
;
_missionController
->
removeAll
();
// Set homeposition of settingsItem.
QmlObjectListModel
*
visuals
=
_missionController
->
visualItems
();
MissionSettingsItem
*
settingsItem
=
visuals
->
value
<
MissionSettingsItem
*>
(
0
);
if
(
settingsItem
==
nullptr
)
{
Q_ASSERT
(
false
);
qWarning
(
"WimaController::updateCurrentMissionItems(): nullptr"
);
return
false
;
}
settingsItem
->
setCoordinate
(
_managerSettings
.
homePosition
());
// Copy mission items to _missionController.
for
(
int
i
=
1
;
i
<
currentMissionItems
.
count
();
i
++
){
auto
*
item
=
currentMissionItems
.
value
<
const
SimpleMissionItem
*>
(
i
);
_missionController
->
insertSimpleMissionItem
(
*
item
,
visuals
->
count
());
}
_masterController
->
sendToVehicle
();
return
true
;
}
void
WimaController
::
removeFromVehicle
()
{
_masterController
->
removeAllFromVehicle
();
_missionController
->
removeAll
();
}
void
WimaController
::
executeSmartRTL
()
{
forceUpload
();
masterController
()
->
managerVehicle
()
->
startMission
();
}
void
WimaController
::
initSmartRTL
()
{
_initSmartRTL
();
}
void
WimaController
::
removeVehicleTrajectoryHistory
()
{
Vehicle
*
managerVehicle
=
masterController
()
->
managerVehicle
();
managerVehicle
->
trajectoryPoints
()
->
clear
();
}
bool
WimaController
::
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
)
{
using
namespace
GeoUtilities
;
using
namespace
PolygonCalculus
;
QPolygonF
polygon2D
;
toCartesianList
(
_joinedArea
.
coordinateList
(),
/*origin*/
start
,
polygon2D
);
QPointF
start2D
(
0
,
0
);
QPointF
end2D
;
toCartesian
(
destination
,
start
,
end2D
);
QVector
<
QPointF
>
path2D
;
bool
retVal
=
PolygonCalculus
::
shortestPath
(
polygon2D
,
start2D
,
end2D
,
path2D
);
toGeoList
(
path2D
,
/*origin*/
start
,
path
);
return
retVal
;
}
bool
WimaController
::
setWimaPlanData
(
const
WimaPlanData
&
planData
)
{
// reset visual items
_areas
.
clear
();
_defaultManager
.
clear
();
_snakeTiles
.
polygons
().
clear
();
_snakeTilesLocal
.
polygons
().
clear
();
_snakeTileCenterPoints
.
clear
();
emit
visualItemsChanged
();
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
emit
snakeTilesChanged
();
emit
snakeTileCenterPointsChanged
();
_localPlanDataValid
=
false
;
// extract list with WimaAreas
QList
<
const
WimaAreaData
*>
areaList
=
planData
.
areaList
();
int
areaCounter
=
0
;
const
int
numAreas
=
4
;
// extract only numAreas Areas, if there are more they are invalid and ignored
for
(
int
i
=
0
;
i
<
areaList
.
size
();
i
++
)
{
const
WimaAreaData
*
areaData
=
areaList
[
i
];
if
(
areaData
->
type
()
==
WimaServiceAreaData
::
typeString
)
{
// is it a service area?
_serviceArea
=
*
qobject_cast
<
const
WimaServiceAreaData
*>
(
areaData
);
areaCounter
++
;
_areas
.
append
(
&
_serviceArea
);
continue
;
}
if
(
areaData
->
type
()
==
WimaMeasurementAreaData
::
typeString
)
{
// is it a measurement area?
_measurementArea
=
*
qobject_cast
<
const
WimaMeasurementAreaData
*>
(
areaData
);
areaCounter
++
;
_areas
.
append
(
&
_measurementArea
);
continue
;
}
if
(
areaData
->
type
()
==
WimaCorridorData
::
typeString
)
{
// is it a corridor?
_corridor
=
*
qobject_cast
<
const
WimaCorridorData
*>
(
areaData
);
areaCounter
++
;
//_visualItems.append(&_corridor); // not needed
continue
;
}
if
(
areaData
->
type
()
==
WimaJoinedAreaData
::
typeString
)
{
// is it a corridor?
_joinedArea
=
*
qobject_cast
<
const
WimaJoinedAreaData
*>
(
areaData
);
areaCounter
++
;
_areas
.
append
(
&
_joinedArea
);
continue
;
}
if
(
areaCounter
>=
numAreas
)
break
;
}
if
(
areaCounter
!=
numAreas
)
{
Q_ASSERT
(
false
);
return
false
;
}
emit
visualItemsChanged
();
// extract mission items
QList
<
MissionItem
>
tempMissionItems
=
planData
.
missionItems
();
if
(
tempMissionItems
.
size
()
<
1
)
{
qWarning
(
"WimaController: Mission items from WimaPlaner empty!"
);
return
false
;
}
for
(
auto
item
:
tempMissionItems
)
{
_defaultManager
.
push_back
(
item
.
coordinate
());
}
_managerSettings
.
setHomePosition
(
QGeoCoordinate
(
_serviceArea
.
center
().
latitude
(),
_serviceArea
.
center
().
longitude
(),
0
)
);
if
(
!
_defaultManager
.
reset
()
){
Q_ASSERT
(
false
);
return
false
;
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
_localPlanDataValid
=
true
;
_initStartSnakeWorker
();
return
true
;
}
WimaController
*
WimaController
::
thisPointer
()
{
return
this
;
}
bool
WimaController
::
_calcNextPhase
()
{
if
(
!
_currentManager
->
next
()
)
{
Q_ASSERT
(
false
);
return
false
;
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
return
true
;
}
bool
WimaController
::
_setStartIndex
()
{
bool
value
;
_currentManager
->
setStartIndex
(
_nextPhaseStartWaypointIndex
.
rawValue
().
toUInt
(
&
value
)
-
1
);
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
return
false
;
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
return
true
;
}
void
WimaController
::
_recalcCurrentPhase
()
{
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateOverlap
()
{
bool
value
;
_currentManager
->
setOverlap
(
_overlapWaypoints
.
rawValue
().
toUInt
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
assert
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateMaxWaypoints
()
{
bool
value
;
_currentManager
->
setN
(
_maxWaypointsPerPhase
.
rawValue
().
toUInt
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateflightSpeed
()
{
bool
value
;
_managerSettings
.
setFlightSpeed
(
_flightSpeed
.
rawValue
().
toDouble
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateArrivalReturnSpeed
()
{
bool
value
;
_managerSettings
.
setArrivalReturnSpeed
(
_arrivalReturnSpeed
.
rawValue
().
toDouble
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateAltitude
()
{
bool
value
;
_managerSettings
.
setAltitude
(
_altitude
.
rawValue
().
toDouble
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_checkBatteryLevel
()
{
Vehicle
*
managerVehicle
=
masterController
()
->
managerVehicle
();
WimaSettings
*
wimaSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
();
int
batteryThreshold
=
wimaSettings
->
lowBatteryThreshold
()
->
rawValue
().
toInt
();
bool
enabled
=
_enableWimaController
.
rawValue
().
toBool
();
unsigned
int
minTime
=
wimaSettings
->
minimalRemainingMissionTime
()
->
rawValue
().
toUInt
();
if
(
managerVehicle
!=
nullptr
&&
enabled
==
true
)
{
Fact
*
battery1percentRemaining
=
managerVehicle
->
battery1FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
Fact
*
battery2percentRemaining
=
managerVehicle
->
battery2FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
if
(
battery1percentRemaining
->
rawValue
().
toDouble
()
<
batteryThreshold
&&
battery2percentRemaining
->
rawValue
().
toDouble
()
<
batteryThreshold
)
{
if
(
!
_lowBatteryHandlingTriggered
)
{
_lowBatteryHandlingTriggered
=
true
;
if
(
!
(
_missionController
->
remainingTime
()
<=
minTime
)
)
{
requestSmartRTL
();
}
}
}
else
{
_lowBatteryHandlingTriggered
=
false
;
}
}
}
void
WimaController
::
_eventTimerHandler
()
{
static
EventTicker
batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
CHECK_BATTERY_INTERVAL
);
static
EventTicker
snakeTicker
(
EVENT_TIMER_INTERVAL
,
SNAKE_EVENT_LOOP_INTERVAL
);
static
EventTicker
nemoStatusTicker
(
EVENT_TIMER_INTERVAL
,
5000
);
// Battery level check necessary?
Fact
*
enableLowBatteryHandling
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
()
->
enableLowBatteryHandling
();
if
(
enableLowBatteryHandling
->
rawValue
().
toBool
()
&&
batteryLevelTicker
.
ready
()
)
_checkBatteryLevel
();
// Snake flight plan update necessary?
// if ( snakeEventLoopTicker.ready() ) {
// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
// }
// }
if
(
nemoStatusTicker
.
ready
()
)
{
this
->
_nemoHeartbeat
.
setStatus
(
_fallbackStatus
);
emit
WimaController
::
nemoStatusChanged
();
emit
WimaController
::
nemoStatusStringChanged
();
}
if
(
snakeTicker
.
ready
()
)
{
if
(
_enableSnake
.
rawValue
().
toBool
()
&&
_pRosBridge
->
connected
()
)
{
if
(
!
_bridgeSetupDone
)
{
qWarning
()
<<
"ROS Bridge setup. "
;
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_pRosBridge
->
start
();
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
qWarning
()
<<
"_pRosBridge->start() time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
<<
" ms"
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
qWarning
()
<<
"_pRosBridge->publish(_snakeOrigin,
\"
/snake/origin
\"
) time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
<<
" ms"
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
qWarning
()
<<
"_pRosBridge->publish(_snakeTilesLocal,
\"
/snake/tiles
\"
) time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
<<
" ms"
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
int
requiredSize
=
this
->
_snakeTilesLocal
.
polygons
().
size
();
auto
&
progress
=
this
->
_nemoProgress
;
if
(
!
this
->
_pRosBridge
->
casePacker
()
->
unpack
(
pDoc
,
progress
)
||
progress
.
progress
().
size
()
!=
requiredSize
)
{
progress
.
progress
().
fill
(
0
,
requiredSize
);
}
emit
WimaController
::
nemoProgressChanged
();
});
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
qWarning
()
<<
"_pRosBridge->subscribe(
\"
/nemo/progress
\"
time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
<<
" ms"
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
/* callback */
[
this
,
&
nemoStatusTicker
](
JsonDocUPtr
pDoc
){
if
(
!
this
->
_pRosBridge
->
casePacker
()
->
unpack
(
pDoc
,
this
->
_nemoHeartbeat
)
)
{
if
(
this
->
_nemoHeartbeat
.
status
()
==
this
->
_fallbackStatus
)
return
;
this
->
_nemoHeartbeat
.
setStatus
(
this
->
_fallbackStatus
);
}
nemoStatusTicker
.
reset
();
this
->
_fallbackStatus
=
-
1
;
/*Timeout*/
emit
WimaController
::
nemoStatusChanged
();
emit
WimaController
::
nemoStatusStringChanged
();
});
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
qWarning
()
<<
"_pRosBridge->subscribe(
\"
/nemo/heartbeat
\"
time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
<<
" ms"
;
_bridgeSetupDone
=
true
;
}
}
else
if
(
_bridgeSetupDone
)
{
_pRosBridge
->
reset
();
_bridgeSetupDone
=
false
;
}
}
}
void
WimaController
::
_smartRTLCleanUp
(
bool
flying
)
{
if
(
!
flying
)
{
// vehicle has landed
_switchWaypointManager
(
_defaultManager
);
_missionController
->
removeAllFromVehicle
();
_missionController
->
removeAll
();
disconnect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
_smartRTLCleanUp
);
}
}
void
WimaController
::
_setPhaseDistance
(
double
distance
)
{
(
void
)
distance
;
// if (!qFuzzyCompare(distance, _phaseDistance)) {
// _phaseDistance = distance;
// emit phaseDistanceChanged();
// }
}
void
WimaController
::
_setPhaseDuration
(
double
duration
)
{
(
void
)
duration
;
// if (!qFuzzyCompare(duration, _phaseDuration)) {
// _phaseDuration = duration;
// emit phaseDurationChanged();
// }
}
bool
WimaController
::
_checkSmartRTLPreCondition
(
QString
&
errorString
)
{
if
(
!
_localPlanDataValid
)
{
errorString
.
append
(
tr
(
"No WiMA data available. Please define at least a measurement and a service area."
));
return
false
;
}
return
_rtlManager
.
checkPrecondition
(
errorString
);
}
void
WimaController
::
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
)
{
if
(
_currentManager
!=
&
manager
)
{
_currentManager
=
&
manager
;
disconnect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateOverlap
);
disconnect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
disconnect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
_maxWaypointsPerPhase
.
setRawValue
(
_currentManager
->
N
());
_overlapWaypoints
.
setRawValue
(
_currentManager
->
overlap
());
_nextPhaseStartWaypointIndex
.
setRawValue
(
_currentManager
->
startIndex
());
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateOverlap
);
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
qWarning
()
<<
"WimaController::_switchWaypointManager: statistics update missing."
;
}
}
void
WimaController
::
_initSmartRTL
()
{
QString
errorString
;
static
int
attemptCounter
=
0
;
attemptCounter
++
;
if
(
_checkSmartRTLPreCondition
(
errorString
)
)
{
_masterController
->
managerVehicle
()
->
pauseVehicle
();
connect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
_smartRTLCleanUp
);
if
(
_rtlManager
.
update
()
)
{
// Calculate return path.
_switchWaypointManager
(
_rtlManager
);
attemptCounter
=
0
;
emit
smartRTLPathConfirm
();
return
;
}
}
else
if
(
attemptCounter
>
SMART_RTL_MAX_ATTEMPTS
)
{
errorString
.
append
(
tr
(
"Smart RTL: No success after maximum number of attempts."
));
qgcApp
()
->
showMessage
(
errorString
);
attemptCounter
=
0
;
}
else
{
_smartRTLTimer
.
singleShot
(
SMART_RTL_ATTEMPT_INTERVAL
,
this
,
&
WimaController
::
_initSmartRTL
);
}
}
void
WimaController
::
_setSnakeCalcInProgress
(
bool
inProgress
)
{
if
(
_snakeCalcInProgress
!=
inProgress
){
_snakeCalcInProgress
=
inProgress
;
emit
snakeCalcInProgressChanged
();
}
}
void
WimaController
::
_updateSnakeData
()
{
_setSnakeCalcInProgress
(
false
);
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_snakeManager
.
clear
();
const
auto
&
r
=
_snakeWorker
.
result
();
if
(
!
r
.
success
)
{
//qgcApp()->showMessage(r.errorMessage);
return
;
}
// create Mission items from r.waypoints
long
n
=
r
.
waypoints
.
size
()
-
r
.
returnPathIdx
.
size
()
-
r
.
arrivalPathIdx
.
size
()
+
2
;
if
(
n
<
1
)
{
return
;
}
// Create QVector<QGeoCoordinate> containing all waypoints;
unsigned
long
startIdx
=
r
.
arrivalPathIdx
.
last
();
unsigned
long
endIdx
=
r
.
returnPathIdx
.
first
();
for
(
unsigned
long
i
=
startIdx
;
i
<=
endIdx
;
++
i
)
{
_snakeManager
.
push_back
(
r
.
waypoints
[
int
(
i
)]);
}
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
double
duration
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
qWarning
()
<<
"WimaController: push_back waypoints execution time: "
<<
duration
<<
" ms."
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_snakeManager
.
update
();
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
duration
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
qWarning
()
<<
"WimaController: _snakeManager.update(); execution time: "
<<
duration
<<
" ms."
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
duration
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
qWarning
()
<<
"WimaController: gui update execution time: "
<<
duration
<<
" ms."
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_snakeOrigin
=
r
.
origin
;
_snakeTileCenterPoints
=
r
.
tileCenterPoints
;
_snakeTiles
=
r
.
tiles
;
_snakeTilesLocal
=
r
.
tilesLocal
;
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
duration
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
qWarning
()
<<
"WimaController: tiles copy execution time: "
<<
duration
<<
" ms."
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
emit
snakeTilesChanged
();
emit
snakeTileCenterPointsChanged
();
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
duration
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
qWarning
()
<<
"WimaController: gui update 2 execution time: "
<<
duration
<<
" ms."
;
}
void
WimaController
::
_initStartSnakeWorker
()
{
if
(
!
_enableSnake
.
rawValue
().
toBool
()
)
return
;
// Stop worker thread if running.
if
(
_snakeWorker
.
isRunning
()
)
{
_snakeWorker
.
quit
();
}
// Initialize _snakeWorker.
_snakeWorker
.
setMeasurementArea
(
_measurementArea
.
coordinateList
());
_snakeWorker
.
setServiceArea
(
_serviceArea
.
coordinateList
());
_snakeWorker
.
setCorridor
(
_corridor
.
coordinateList
());
_snakeWorker
.
setProgress
(
_nemoProgress
.
progress
());
_snakeWorker
.
setLineDistance
(
_snakeLineDistance
.
rawValue
().
toDouble
());
_snakeWorker
.
setMinTransectLength
(
_snakeMinTransectLength
.
rawValue
().
toDouble
());
_snakeWorker
.
setTileHeight
(
_snakeTileHeight
.
rawValue
().
toDouble
());
_snakeWorker
.
setTileWidth
(
_snakeTileWidth
.
rawValue
().
toDouble
());
_snakeWorker
.
setMinTileArea
(
_snakeMinTileArea
.
rawValue
().
toDouble
());
_setSnakeCalcInProgress
(
true
);
// Start worker thread.
_snakeWorker
.
start
();
}
void
WimaController
::
_switchSnakeManager
(
QVariant
variant
)
{
if
(
variant
.
value
<
bool
>
()){
_switchWaypointManager
(
_snakeManager
);
}
else
{
_switchWaypointManager
(
_defaultManager
);
}
}
src/Wima/WimaController_new.h
deleted
100644 → 0
View file @
9a7d98f8
#
pragma
once
#include <QObject>
#include <QScopedPointer>
#include "QGCMapPolygon.h"
#include "QmlObjectListModel.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaServiceAreaData.h"
#include "WimaPlanData.h"
#include "PlanMasterController.h"
#include "MissionController.h"
#include "SurveyComplexItem.h"
#include "SimpleMissionItem.h"
#include "MissionSettingsItem.h"
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "SettingsFact.h"
#include "WimaSettings.h"
#include "SettingsManager.h"
#include "snake.h"
#include "Snake/SnakeWorker.h"
#include "Snake/GeoPolygonArray.h"
#include "Snake/PolygonArray.h"
#include "Geometry/GeoPoint3D.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "ros_bridge/include/ROSBridge.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"
#include <map>
#define CHECK_BATTERY_INTERVAL 1000 // ms
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
using
namespace
snake
;
typedef
std
::
unique_ptr
<
rapidjson
::
Document
>
JsonDocUPtr
;
class
WimaController
:
public
QObject
{
Q_OBJECT
enum
FileType
{
WimaFile
,
PlanFile
};
typedef
QScopedPointer
<
ROSBridge
::
ROSBridge
>
ROSBridgePtr
;
public:
WimaController
(
QObject
*
parent
=
nullptr
);
// Controllers.
Q_PROPERTY
(
PlanMasterController
*
masterController
READ
masterController
WRITE
setMasterController
NOTIFY
masterControllerChanged
)
Q_PROPERTY
(
MissionController
*
missionController
READ
missionController
WRITE
setMissionController
NOTIFY
missionControllerChanged
)
// Wima Data.
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visualItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
READ
missionItems
NOTIFY
missionItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
currentMissionItems
READ
currentMissionItems
NOTIFY
currentMissionItemsChanged
)
Q_PROPERTY
(
QVariantList
waypointPath
READ
waypointPath
NOTIFY
waypointPathChanged
)
Q_PROPERTY
(
QVariantList
currentWaypointPath
READ
currentWaypointPath
NOTIFY
currentWaypointPathChanged
)
Q_PROPERTY
(
Fact
*
enableWimaController
READ
enableWimaController
CONSTANT
)
// Waypoint navigaton.
Q_PROPERTY
(
Fact
*
overlapWaypoints
READ
overlapWaypoints
CONSTANT
)
Q_PROPERTY
(
Fact
*
maxWaypointsPerPhase
READ
maxWaypointsPerPhase
CONSTANT
)
Q_PROPERTY
(
Fact
*
startWaypointIndex
READ
startWaypointIndex
CONSTANT
)
Q_PROPERTY
(
Fact
*
showAllMissionItems
READ
showAllMissionItems
CONSTANT
)
Q_PROPERTY
(
Fact
*
showCurrentMissionItems
READ
showCurrentMissionItems
CONSTANT
)
// Waypoint settings.
Q_PROPERTY
(
Fact
*
flightSpeed
READ
flightSpeed
CONSTANT
)
Q_PROPERTY
(
Fact
*
altitude
READ
altitude
CONSTANT
)
Q_PROPERTY
(
Fact
*
arrivalReturnSpeed
READ
arrivalReturnSpeed
CONSTANT
)
// Waypoint statistics.
Q_PROPERTY
(
double
phaseDistance
READ
phaseDistance
NOTIFY
phaseDistanceChanged
)
Q_PROPERTY
(
double
phaseDuration
READ
phaseDuration
NOTIFY
phaseDurationChanged
)
// Snake
Q_PROPERTY
(
Fact
*
enableSnake
READ
enableSnake
CONSTANT
)
Q_PROPERTY
(
int
nemoStatus
READ
nemoStatus
NOTIFY
nemoStatusChanged
)
Q_PROPERTY
(
QString
nemoStatusString
READ
nemoStatusString
NOTIFY
nemoStatusStringChanged
)
Q_PROPERTY
(
bool
snakeCalcInProgress
READ
snakeCalcInProgress
NOTIFY
snakeCalcInProgressChanged
)
Q_PROPERTY
(
Fact
*
snakeTileWidth
READ
snakeTileWidth
CONSTANT
)
Q_PROPERTY
(
Fact
*
snakeTileHeight
READ
snakeTileHeight
CONSTANT
)
Q_PROPERTY
(
Fact
*
snakeMinTileArea
READ
snakeMinTileArea
CONSTANT
)
Q_PROPERTY
(
Fact
*
snakeLineDistance
READ
snakeLineDistance
CONSTANT
)
Q_PROPERTY
(
Fact
*
snakeMinTransectLength
READ
snakeMinTransectLength
CONSTANT
)
Q_PROPERTY
(
QmlObjectListModel
*
snakeTiles
READ
snakeTiles
NOTIFY
snakeTilesChanged
)
Q_PROPERTY
(
QVariantList
snakeTileCenterPoints
READ
snakeTileCenterPoints
NOTIFY
snakeTileCenterPointsChanged
)
Q_PROPERTY
(
QVector
<
int
>
nemoProgress
READ
nemoProgress
NOTIFY
nemoProgressChanged
)
// Property accessors
// Controllers.
PlanMasterController
*
masterController
(
void
);
MissionController
*
missionController
(
void
);
// Wima Data
QmlObjectListModel
*
visualItems
(
void
);
QGCMapPolygon
joinedArea
(
void
)
const
;
// Waypoints.
QmlObjectListModel
*
missionItems
(
void
);
QmlObjectListModel
*
currentMissionItems
(
void
);
QVariantList
waypointPath
(
void
);
QVariantList
currentWaypointPath
(
void
);
// Settings facts.
Fact
*
enableWimaController
(
void
);
Fact
*
overlapWaypoints
(
void
);
Fact
*
maxWaypointsPerPhase
(
void
);
Fact
*
startWaypointIndex
(
void
);
Fact
*
showAllMissionItems
(
void
);
Fact
*
showCurrentMissionItems
(
void
);
Fact
*
flightSpeed
(
void
);
Fact
*
arrivalReturnSpeed
(
void
);
Fact
*
altitude
(
void
);
// Snake settings facts.
Fact
*
enableSnake
(
void
)
{
return
&
_enableSnake
;
}
Fact
*
snakeTileWidth
(
void
)
{
return
&
_snakeTileWidth
;}
Fact
*
snakeTileHeight
(
void
)
{
return
&
_snakeTileHeight
;}
Fact
*
snakeMinTileArea
(
void
)
{
return
&
_snakeMinTileArea
;}
Fact
*
snakeLineDistance
(
void
)
{
return
&
_snakeLineDistance
;}
Fact
*
snakeMinTransectLength
(
void
)
{
return
&
_snakeMinTransectLength
;}
// Snake data.
QmlObjectListModel
*
snakeTiles
(
void
)
{
return
_snakeTiles
.
QmlObjectListModel
();}
QVariantList
snakeTileCenterPoints
(
void
)
{
return
_snakeTileCenterPoints
;}
QVector
<
int
>
nemoProgress
(
void
);
int
nemoStatus
(
void
)
const
;
QString
nemoStatusString
(
void
)
const
;
bool
snakeCalcInProgress
(
void
)
const
;
// Smart RTL.
bool
uploadOverrideRequired
(
void
)
const
;
bool
vehicleHasLowBattery
(
void
)
const
;
// Waypoint statistics.
double
phaseDistance
(
void
)
const
;
double
phaseDuration
(
void
)
const
;
// Property setters
void
setMasterController
(
PlanMasterController
*
masterController
);
void
setMissionController
(
MissionController
*
missionController
);
bool
setWimaPlanData
(
const
WimaPlanData
&
planData
);
// Member Methodes
Q_INVOKABLE
WimaController
*
thisPointer
();
// Waypoint navigation.
Q_INVOKABLE
void
nextPhase
();
Q_INVOKABLE
void
previousPhase
();
Q_INVOKABLE
void
resetPhase
();
// Smart RTL.
Q_INVOKABLE
void
requestSmartRTL
();
Q_INVOKABLE
void
initSmartRTL
();
Q_INVOKABLE
void
executeSmartRTL
();
// Other.
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
Q_INVOKABLE
bool
upload
();
Q_INVOKABLE
bool
forceUpload
();
Q_INVOKABLE
void
removeFromVehicle
();
// static Members
static
const
char
*
areaItemsName
;
static
const
char
*
missionItemsName
;
static
const
char
*
settingsGroup
;
static
const
char
*
endWaypointIndexName
;
static
const
char
*
enableWimaControllerName
;
static
const
char
*
overlapWaypointsName
;
static
const
char
*
maxWaypointsPerPhaseName
;
static
const
char
*
startWaypointIndexName
;
static
const
char
*
showAllMissionItemsName
;
static
const
char
*
showCurrentMissionItemsName
;
static
const
char
*
flightSpeedName
;
static
const
char
*
arrivalReturnSpeedName
;
static
const
char
*
altitudeName
;
static
const
char
*
snakeTileWidthName
;
static
const
char
*
snakeTileHeightName
;
static
const
char
*
snakeMinTileAreaName
;
static
const
char
*
snakeLineDistanceName
;
static
const
char
*
snakeMinTransectLengthName
;
signals:
// Controllers.
void
masterControllerChanged
(
void
);
void
missionControllerChanged
(
void
);
// Wima data.
void
visualItemsChanged
(
void
);
// Waypoints.
void
missionItemsChanged
(
void
);
void
currentMissionItemsChanged
(
void
);
void
waypointPathChanged
(
void
);
void
currentWaypointPathChanged
(
void
);
// Smart RTL.
void
smartRTLRequestConfirm
(
void
);
void
smartRTLPathConfirm
(
void
);
// Upload.
void
forceUploadConfirm
(
void
);
// Waypoint statistics.
void
phaseDistanceChanged
(
void
);
void
phaseDurationChanged
(
void
);
// Snake.
void
snakeConnectionStatusChanged
(
void
);
void
snakeCalcInProgressChanged
(
void
);
void
snakeTilesChanged
(
void
);
void
snakeTileCenterPointsChanged
(
void
);
void
nemoProgressChanged
(
void
);
void
nemoStatusChanged
(
void
);
void
nemoStatusStringChanged
(
void
);
private
slots
:
// Waypoint navigation / helper.
bool
_calcNextPhase
(
void
);
void
_recalcCurrentPhase
(
void
);
bool
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
);
// Slicing parameters
bool
_setStartIndex
(
void
);
void
_updateOverlap
(
void
);
void
_updateMaxWaypoints
(
void
);
// Waypoint settings.
void
_updateflightSpeed
(
void
);
void
_updateArrivalReturnSpeed
(
void
);
void
_updateAltitude
(
void
);
// Waypoint Statistics.
void
_setPhaseDistance
(
double
distance
);
void
_setPhaseDuration
(
double
duration
);
// SMART RTL
void
_checkBatteryLevel
(
void
);
bool
_checkSmartRTLPreCondition
(
QString
&
errorString
);
void
_initSmartRTL
();
void
_smartRTLCleanUp
(
bool
flying
);
// Snake.
void
_setSnakeCalcInProgress
(
bool
inProgress
);
void
_updateSnakeData
();
void
_initStartSnakeWorker
();
void
_switchSnakeManager
(
QVariant
variant
);
// Periodic tasks.
void
_eventTimerHandler
(
void
);
// Waypoint manager handling.
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
private:
using
StatusMap
=
std
::
map
<
int
,
QString
>
;
// Controllers.
PlanMasterController
*
_masterController
;
MissionController
*
_missionController
;
// Wima Data.
QmlObjectListModel
_areas
;
// contains all visible areas
WimaJoinedAreaData
_joinedArea
;
// joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData
_measurementArea
;
// measurement area
WimaServiceAreaData
_serviceArea
;
// area for supplying
WimaCorridorData
_corridor
;
// corridor connecting opArea and serArea
bool
_localPlanDataValid
;
// Waypoint Managers.
WaypointManager
::
AreaInterface
_areaInterface
;
WaypointManager
::
Settings
_managerSettings
;
WaypointManager
::
DefaultManager
_defaultManager
;
WaypointManager
::
DefaultManager
_snakeManager
;
WaypointManager
::
RTLManager
_rtlManager
;
WaypointManager
::
ManagerBase
*
_currentManager
;
using
ManagerList
=
QList
<
WaypointManager
::
ManagerBase
*>
;
ManagerList
_managerList
;
// Settings Facts.
QMap
<
QString
,
FactMetaData
*>
_metaDataMap
;
SettingsFact
_enableWimaController
;
// enables or disables the wimaControler
SettingsFact
_overlapWaypoints
;
// determines the number of overlapping waypoints between two consecutive mission phases
SettingsFact
_maxWaypointsPerPhase
;
// determines the maximum number waypoints per phase
SettingsFact
_nextPhaseStartWaypointIndex
;
// index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
// defining the first element of the next phase
SettingsFact
_showAllMissionItems
;
// bool value, Determines whether the mission items of the overall mission are displayed or not.
SettingsFact
_showCurrentMissionItems
;
// bool value, Determines whether the mission items of the current mission phase are displayed or not.
SettingsFact
_flightSpeed
;
// mission flight speed
SettingsFact
_arrivalReturnSpeed
;
// arrival and return path speed
SettingsFact
_altitude
;
// mission altitude
SettingsFact
_enableSnake
;
// Enable Snake (see snake.h)
SettingsFact
_snakeTileWidth
;
SettingsFact
_snakeTileHeight
;
SettingsFact
_snakeMinTileArea
;
SettingsFact
_snakeLineDistance
;
SettingsFact
_snakeMinTransectLength
;
// Smart RTL.
QTimer
_smartRTLTimer
;
bool
_lowBatteryHandlingTriggered
;
// Waypoint statistics.
double
_measurementPathLength
;
// the lenght of the phase in meters
// double _phaseDistance; // the lenth in meters of the current phase
// double _phaseDuration; // the phase duration in seconds
// Snake
bool
_snakeCalcInProgress
;
SnakeDataManager
_snakeWorker
;
GeoPoint
_snakeOrigin
;
GeoPolygonArray
_snakeTiles
;
// tiles
PolygonArray
_snakeTilesLocal
;
// tiles local coordinate system
QVariantList
_snakeTileCenterPoints
;
QNemoProgress
_nemoProgress
;
// measurement progress
QNemoHeartbeat
_nemoHeartbeat
;
// measurement progress
int
_fallbackStatus
;
ROSBridgePtr
_pRosBridge
;
bool
_bridgeSetupDone
;
static
StatusMap
_nemoStatusMap
;
// Periodic tasks.
QTimer
_eventTimer
;
};
src/comm/ros_bridge/include/CasePacker.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/MessageTag.h"
#include "ros_bridge/include/TypeFactory.h"
#include "ros_bridge/include/JsonFactory.h"
#include <memory>
#include "rapidjson/include/rapidjson/document.h"
namespace
ROSBridge
{
class
CasePacker
{
typedef
MessageTag
Tag
;
typedef
rapidjson
::
Document
JsonDoc
;
typedef
std
::
unique_ptr
<
JsonDoc
>
JsonDocUPtr
;
public:
CasePacker
()
=
delete
;
CasePacker
(
TypeFactory
*
typeFactory
,
JsonFactory
*
jsonFactory
);
template
<
class
T
>
JsonDocUPtr
pack
(
const
T
&
msg
,
const
std
::
string
&
topic
)
const
{
JsonDocUPtr
docPt
(
_jsonFactory
->
create
(
msg
));
std
::
string
messageType
=
T
::
Group
::
messageType
();
addTag
(
docPt
,
topic
,
messageType
.
c_str
());
return
docPt
;
}
template
<
class
T
>
bool
unpack
(
JsonDocUPtr
&
pDoc
,
T
&
msg
)
const
{
removeTag
(
pDoc
);
return
_typeFactory
->
create
(
*
pDoc
.
get
(),
msg
);
}
bool
getTag
(
const
JsonDocUPtr
&
pDoc
,
Tag
&
tag
)
const
;
void
addTag
(
JsonDocUPtr
&
doc
,
const
std
::
string
&
topic
,
const
std
::
string
&
messageType
)
const
;
void
addTag
(
JsonDocUPtr
&
doc
,
const
Tag
&
tag
)
const
;
void
removeTag
(
JsonDocUPtr
&
pDoc
)
const
;
bool
getTopic
(
const
JsonDocUPtr
&
pDoc
,
std
::
string
&
topic
)
const
;
bool
getMessageType
(
const
JsonDocUPtr
&
pDoc
,
std
::
string
&
messageType
)
const
;
static
const
char
*
topicKey
;
static
const
char
*
messageTypeKey
;
private:
TypeFactory
*
_typeFactory
;
JsonFactory
*
_jsonFactory
;
};
}
src/comm/ros_bridge/include/ComPrivateInclude.cpp
deleted
100644 → 0
View file @
9a7d98f8
#include "ros_bridge/include/ComPrivateInclude.h"
#include <functional>
std
::
size_t
ROSBridge
::
ComPrivate
::
getHash
(
const
std
::
string
&
str
)
{
std
::
hash
<
std
::
string
>
hash
;
return
hash
(
str
);
}
std
::
size_t
ROSBridge
::
ComPrivate
::
getHash
(
const
char
*
str
)
{
return
ROSBridge
::
ComPrivate
::
getHash
(
std
::
string
(
str
));
}
src/comm/ros_bridge/include/GenericMessages.h
deleted
100644 → 0
View file @
9a7d98f8
#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
)
{}
Time
(
const
Time
&
time
)
:
_secs
(
time
.
secs
()),
_nsecs
(
time
.
nSecs
())
{}
uint32_t
secs
()
const
{
return
_secs
;}
uint32_t
nSecs
()
const
{
return
_nsecs
;}
void
setSecs
(
uint32_t
secs
)
{
_secs
=
secs
;}
void
setNSecs
(
uint32_t
nsecs
)
{
_nsecs
=
nsecs
;}
private:
uint32_t
_secs
;
uint32_t
_nsecs
;
};
//! @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
)
{}
Header
(
const
Header
&
header
)
:
_seq
(
header
.
seq
())
,
_stamp
(
header
.
stamp
())
,
_frameId
(
header
.
frameId
())
{}
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
);}
GenericPolygon
&
operator
=
(
const
GenericPolygon
&
other
)
{
this
->
_points
=
other
.
_points
;
return
*
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
)
{}
~
GeoPoint
()
override
{}
_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
MessageBaseClass
{
public:
typedef
MessageGroups
::
ProgressGroup
Group
;
GenericProgress
()
:
MessageBaseClass
()
{}
GenericProgress
(
const
ContainterType
<
IntType
>
&
progress
)
:
MessageBaseClass
(),
_progress
(
progress
){}
GenericProgress
(
const
GenericProgress
&
p
)
:
MessageBaseClass
(),
_progress
(
p
.
progress
()){}
~
GenericProgress
()
{}
virtual
GenericProgress
*
Clone
()
const
override
{
return
new
GenericProgress
(
*
this
);
}
virtual
const
ContainterType
<
IntType
>
&
progress
(
void
)
const
{
return
_progress
;}
virtual
ContainterType
<
IntType
>
&
progress
(
void
)
{
return
_progress
;}
protected:
ContainterType
<
IntType
>
_progress
;
};
typedef
GenericProgress
<>
Progress
;
// ==============================================================================
// class Heartbeat
// ==============================================================================
//! @brief C++ representation of nemo_msgs/Heartbeat message
class
Heartbeat
:
public
MessageBaseClass
{
public:
typedef
MessageGroups
::
HeartbeatGroup
Group
;
Heartbeat
(){}
Heartbeat
(
int
status
)
:
_status
(
status
){}
Heartbeat
(
const
Heartbeat
&
heartbeat
)
:
MessageBaseClass
(),
_status
(
heartbeat
.
status
()){}
~
Heartbeat
()
=
default
;
virtual
Heartbeat
*
Clone
()
const
override
{
return
new
Heartbeat
(
*
this
);
}
virtual
int
status
(
void
)
const
{
return
_status
;}
virtual
void
setStatus
(
int
status
)
{
_status
=
status
;}
protected:
int
_status
;
};
typedef
GenericProgress
<>
Progress
;
}
// namespace NemoMsgs
}
// namespace GenericMessages
}
// namespace ROSBridge
src/comm/ros_bridge/include/JsonFactory.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "MessageBaseClass.h"
#include "utilities.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"
namespace
ROSBridge
{
class
StdHeaderPolicy
;
//!
//! \brief The JsonFactory class is used to create ROS messages.
//!
//! 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
HeaderPolicy
=
StdHeaderPolicy
>
class
GenericJsonFactory
:
public
HeaderPolicy
{
typedef
MessageBaseClass
ROSMsg
;
public:
GenericJsonFactory
()
:
HeaderPolicy
()
{}
//!
//! \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
>
rapidjson
::
Document
*
create
(
const
T
&
msg
){
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
>
());
}
private:
// ===========================================================================
// EmptyGroup and not implemented Groups
// ===========================================================================
template
<
class
U
,
class
V
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
V
>
){
(
void
)
msg
;
assert
(
false
);
// Implementation missing for group U::Group!
return
nullptr
;
}
// ===========================================================================
// Point32Group
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
Point32Group
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
Point32
::
toJson
<
_Float32
>
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
// ===========================================================================
// GeoPointGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
GeoPointGroup
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
GeographicMsgs
::
GeoPoint
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
// ===========================================================================
// PolygonGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
PolygonGroup
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
Polygon
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
// ===========================================================================
// PolygonStampedGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
PolygonStampedGroup
>
){
using
namespace
ROSBridge
;
using
namespace
ROSBridge
::
traits
;
typedef
HasMemberHeader
<
U
>
HasHeader
;
return
_createPolygonStamped
(
msg
,
Int2Type
<
HasHeader
::
value
>
());
}
template
<
class
U
,
int
k
>
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
::
GeometryMsgs
::
PolygonStamped
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
template
<
class
U
>
rapidjson
::
Document
*
_createPolygonStamped
(
const
U
&
msg
,
Int2Type
<
0
>
){
// U has no member header(), generate one on the fly.
using
namespace
ROSBridge
;
GenericMessages
::
StdMsgs
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
PolygonStamped
::
toJson
(
msg
.
polygon
(),
header
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
// ===========================================================================
// PolygonArrayGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
PolygonArrayGroup
>
){
using
namespace
ROSBridge
;
using
namespace
ROSBridge
::
traits
;
typedef
HasMemberHeader
<
U
>
HasHeader
;
return
_createPolygonArray
(
msg
,
Int2Type
<
HasHeader
::
value
>
());
}
template
<
class
U
,
int
k
>
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
::
JSKRecognitionMsgs
::
PolygonArray
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
template
<
class
U
>
rapidjson
::
Document
*
_createPolygonArray
(
const
U
&
msg
,
Int2Type
<
0
>
){
// U has no member header(), generate one on the fly.
using
namespace
ROSBridge
;
GenericMessages
::
StdMsgs
::
Header
header
(
HeaderPolicy
::
header
(
msg
));
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
JSKRecognitionMsgs
::
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
;
}
// ===========================================================================
// HeartbeatGroup
// ===========================================================================
template
<
class
U
>
rapidjson
::
Document
*
_create
(
const
U
&
msg
,
Type2Type
<
MessageGroups
::
HeartbeatGroup
>
){
using
namespace
ROSBridge
;
rapidjson
::
Document
*
doc
=
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
);
bool
ret
=
JsonMethodes
::
NemoMsgs
::
Heartbeat
::
toJson
(
msg
,
*
doc
,
doc
->
GetAllocator
());
assert
(
ret
);
(
void
)
ret
;
return
doc
;
}
};
class
StdHeaderPolicy
{
typedef
ROSBridge
::
GenericMessages
::
StdMsgs
::
Header
Header
;
typedef
ROSBridge
::
GenericMessages
::
StdMsgs
::
Time
Time
;
public:
StdHeaderPolicy
()
:
_seq
(
-
1
){}
//!
//! \brief header Returns the header belonging to msg.
//! \return Returns the header belonging to msg.
template
<
class
T
>
Header
header
(
const
T
&
msg
)
{
return
Header
(
std
::
uint32_t
(
++
_seq
),
time
(
msg
),
"/map"
);
}
//!
//! \brief time Returns the current time.
//! \return Returns the current time.
template
<
class
T
>
Time
time
(
const
T
&
msg
)
{
(
void
)
msg
;
return
Time
(
0
,
0
);
}
private:
long
_seq
;
};
typedef
GenericJsonFactory
<>
JsonFactory
;
}
// end namespace ros_bridge
src/comm/ros_bridge/include/JsonMethodes.h
deleted
100644 → 0
View file @
9a7d98f8
#ifndef MESSAGES_H
#define MESSAGES_H
#include <iostream>
#include <vector>
#include <boost/type_traits/remove_cv.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include "ros_bridge/rapidjson/include/rapidjson/rapidjson.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "utilities.h"
#include "MessageTraits.h"
#include <ostream>
namespace
ROSBridge
{
//! @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
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"secs"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
time
.
secs
())),
allocator
);
value
.
AddMember
(
"nsecs"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
time
.
nSecs
())),
allocator
);
return
true
;
}
template
<
class
TimeType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
TimeType
&
time
)
{
if
(
!
value
.
HasMember
(
"secs"
)
||
!
value
[
"secs"
].
IsUint
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"nsecs"
)
||
!
value
[
"nsecs"
].
IsUint
()){
assert
(
false
);
return
false
;
}
time
.
setSecs
(
value
[
"secs"
].
GetUint
());
time
.
setNSecs
(
value
[
"nsecs"
].
GetUint
());
return
true
;
}
}
// Time
//! @brief Namespace containing methodes for std_msgs/Header message generation.
namespace
Header
{
template
<
class
HeaderType
>
bool
toJson
(
const
HeaderType
&
header
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"seq"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
header
.
seq
())),
allocator
);
rapidjson
::
Value
stamp
(
rapidjson
::
kObjectType
);
if
(
!
Time
::
toJson
(
header
.
stamp
(),
stamp
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"stamp"
,
stamp
,
allocator
);
value
.
AddMember
(
"frame_id"
,
rapidjson
::
Value
().
SetString
(
header
.
frameId
().
data
(),
header
.
frameId
().
length
(),
allocator
),
allocator
);
return
true
;
}
template
<
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
HeaderType
&
header
)
{
if
(
!
value
.
HasMember
(
"seq"
)
||
!
value
[
"seq"
].
IsUint
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"stamp"
)){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"frame_id"
)
||
!
value
[
"frame_id"
].
IsString
()){
assert
(
false
);
return
false
;
}
header
.
setSeq
(
value
[
"seq"
].
GetUint
());
decltype
(
header
.
stamp
())
time
;
if
(
!
Time
::
fromJson
(
value
[
"stamp"
],
time
)){
assert
(
false
);
return
false
;
}
header
.
setStamp
(
time
);
header
.
setFrameId
(
value
[
"frame_id"
].
GetString
());
return
true
;
}
}
// Header
}
// StdMsgs
//! @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
;
namespace
det
{
//detail
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has3Components
>
)
{
return
p
.
z
();
}
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
// p has no member z() -> add 0.
}
template
<
class
T
,
typename
V
>
bool
setZ
(
const
rapidjson
::
Value
&
doc
,
const
T
&
p
,
Type2Type
<
V
>
)
{
p
.
setZ
(
doc
[
"z"
].
GetFloat
());
return
true
;
}
template
<
class
T
>
bool
setZ
(
const
rapidjson
::
Value
&
doc
,
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
p
;
return
true
;
}
}
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
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.
value
.
AddMember
(
"z"
,
rapidjson
::
Value
().
SetFloat
(
z
),
allocator
);
return
true
;
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
if
(
!
value
.
HasMember
(
"x"
)
||
!
value
[
"x"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"y"
)
||
!
value
[
"y"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"z"
)
||
!
value
[
"z"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setX
(
value
[
"x"
].
GetFloat
());
p
.
setY
(
value
[
"y"
].
GetFloat
());
typedef
typename
Select
<
HasMemberSetZ
<
PointType
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
(
void
)
det
::
setZ
(
value
[
"z"
],
p
,
Type2Type
<
Components
>
());
// If PointType has no member z() discard doc["z"].
return
true
;
}
}
// Point32
//! @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
<
std
::
uint64_t
(
poly
.
points
().
size
());
++
i
)
{
rapidjson
::
Document
point
(
rapidjson
::
kObjectType
);
if
(
!
Point32
::
toJson
(
poly
.
points
()[
i
],
point
,
allocator
)
){
assert
(
false
);
return
false
;
}
points
.
PushBack
(
point
,
allocator
);
}
value
.
AddMember
(
"points"
,
points
,
allocator
);
return
true
;
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonType
&
poly
)
{
if
(
!
value
.
HasMember
(
"points"
)
||
!
value
[
"points"
].
IsArray
()){
assert
(
false
);
return
false
;
}
const
auto
&
jsonArray
=
value
[
"points"
].
GetArray
();
poly
.
points
().
clear
();
poly
.
points
().
reserve
(
jsonArray
.
Size
());
typedef
decltype
(
poly
.
points
()[
0
])
PointTypeCVR
;
typedef
typename
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
);
}
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
);
return
true
;
}
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
;
}
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
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
Type2Type
<
Has3Components
>
)
{
return
p
.
altitude
();
}
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
}
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
Type2Type
<
Has3Components
>
)
{
p
.
setAltitude
(
doc
.
GetFloat
());
}
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
p
;
}
}
// namespace det
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
value
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
typedef
typename
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;
value
.
AddMember
(
"altitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
altitude
),
allocator
);
return
true
;
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
if
(
!
value
.
HasMember
(
"latitude"
)
||
!
value
[
"latitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"longitude"
)
||
!
value
[
"longitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"altitude"
)
||
!
value
[
"altitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setLatitude
(
value
[
"latitude"
].
GetFloat
());
p
.
setLongitude
(
value
[
"longitude"
].
GetFloat
());
typedef
typename
Select
<
HasMemberSetAltitude
<
PointType
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
det
::
setAltitude
(
value
[
"altitude"
],
p
,
Type2Type
<
Components
>
());
// If T has no member altitude() discard doc["altitude"];
return
true
;
}
}
// GeoPoint
}
// GeographicMsgs
//! @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
;
using
namespace
ROSBridge
::
JsonMethodes
::
StdMsgs
;
using
namespace
ROSBridge
::
JsonMethodes
::
GeometryMsgs
;
namespace
PAdetail
{
//! Helper functions to generate Json entries for labels and likelihood.
//! \note \p p has member \fn labels().
template
<
class
PolygonArrayType
,
int
k
>
void
labelsToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
labels
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Int2Type
<
k
>
){
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
p
.
labels
().
size
();
++
i
)
labels
.
PushBack
(
rapidjson
::
Value
().
SetUint
(
p
.
labels
()[
i
]),
allocator
);
}
//! \note \p p has no member \fn labels().
template
<
class
PolygonArrayType
>
void
labelsToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
labels
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Int2Type
<
0
>
){
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)(
p
.
polygons
().
size
());
++
i
)
labels
.
PushBack
(
rapidjson
::
Value
().
SetUint
(
0
),
allocator
);
// use zero!
}
//! \note \p p has member \fn likelihood().
template
<
class
PolygonArrayType
,
int
k
>
void
likelihoodToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
likelyhood
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Int2Type
<
k
>
){
p
.
likelyhood
().
clear
();
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
p
.
likelyhood
().
size
();
++
i
)
likelyhood
.
PushBack
(
rapidjson
::
Value
().
SetFloat
(
p
.
likelyhood
()[
i
]),
allocator
);
}
//! \note \p p has no member \fn likelihood().
template
<
class
PolygonArrayType
>
void
likelihoodToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
likelyhood
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
Int2Type
<
0
>
){
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
>
){
p
.
labels
().
clear
();
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
;
}
}
//!
//! Create PolygonArray message from \p p. \p p contains a header.
//! \param p Class implementing the PolygonArrayType interface.
//! \param doc Rapidjson document used to store the PolygonArray message.
//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
//!
//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
//! will be replaced by arrays filled with zero and size p.polygons.size().
//!
//! \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
&
pArray
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
Header
::
toJson
(
pArray
.
header
(),
header
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"header"
,
header
,
allocator
);
rapidjson
::
Value
polygons
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
pArray
.
polygons
().
size
();
++
i
){
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
PolygonStamped
::
toJson
(
pArray
.
polygons
()[
i
],
polygon
,
allocator
)){
assert
(
false
);
return
false
;
}
polygons
.
PushBack
(
polygon
,
allocator
);
}
value
.
AddMember
(
"polygons"
,
polygons
,
allocator
);
rapidjson
::
Value
labels
(
rapidjson
::
kArrayType
);
typedef
HasMemberLabels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
labelsToJson
(
pArray
,
labels
,
allocator
,
Int2Type
<
HasLabels
::
value
>
());
value
.
AddMember
(
"labels"
,
labels
,
allocator
);
rapidjson
::
Value
likelihood
(
rapidjson
::
kArrayType
);
typedef
HasMemberLikelihood
<
PolygonArrayType
>
HasLikelihood
;
PAdetail
::
likelihoodToJson
(
pArray
,
likelihood
,
allocator
,
Int2Type
<
HasLikelihood
::
value
>
());
value
.
AddMember
(
"likelihood"
,
likelihood
,
allocator
);
return
true
;
}
//!
//! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
//! \param p Class implementing the PolygonArrayType interface.
//! \param h Class implementing the HeaderType interface.
//! \param doc Rapidjson document used to store the PolygonArray message.
//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
//!
//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
//! will be replaced by arrays filled with zero and size p.polygons.size().
//!
//! \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
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
Header
::
toJson
(
h
,
header
,
allocator
)){
assert
(
false
);
return
false
;
}
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
].
polygon
(),
h
,
polygon
,
allocator
)){
assert
(
false
);
return
false
;
}
polygons
.
PushBack
(
polygon
,
allocator
);
}
value
.
AddMember
(
"polygons"
,
polygons
,
allocator
);
rapidjson
::
Value
labels
(
rapidjson
::
kArrayType
);
typedef
HasMemberLabels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
labelsToJson
(
p
,
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
>
());
value
.
AddMember
(
"likelihood"
,
likelihood
,
allocator
);
return
true
;
}
template
<
class
PolygonArrayType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonArrayType
&
p
)
{
if
(
!
value
.
HasMember
(
"header"
)){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"polygons"
)
||
!
value
[
"polygons"
].
IsArray
()
){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"labels"
)
||
!
value
[
"labels"
].
IsArray
()
){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"likelihood"
)
||
!
value
[
"likelihood"
].
IsArray
()
){
assert
(
false
);
return
false
;
}
typedef
traits
::
HasMemberHeader
<
PolygonArrayType
>
HasHeader
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
value
[
"header"
],
p
,
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
const
auto
&
polyStampedJson
=
value
[
"polygons"
];
p
.
polygons
().
clear
();
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
(
unsigned
int
i
=
0
;
i
<
polyStampedJson
.
Size
();
++
i
)
{
if
(
!
polyStampedJson
[
i
].
HasMember
(
"header"
)
){
assert
(
false
);
return
false
;
}
PolyStamped
polyStamped
;
if
(
!
PolygonStamped
::
det
::
setHeader
(
polyStampedJson
[
i
][
"header"
],
polyStamped
,
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
if
(
!
Polygon
::
fromJson
(
polyStampedJson
[
i
][
"polygon"
],
polyStamped
.
polygon
())){
assert
(
false
);
return
false
;
}
p
.
polygons
().
push_back
(
std
::
move
(
polyStamped
));
}
typedef
traits
::
HasMemberLabels
<
PolygonArrayType
>
HasLabels
;
PAdetail
::
setLabels
(
value
[
"labels"
],
p
,
Int2Type
<
HasLabels
::
value
>
());
typedef
traits
::
HasMemberLikelihood
<
PolygonArrayType
>
HasLikelihood
;
PAdetail
::
setLikelihood
(
value
[
"likelihood"
],
p
,
Int2Type
<
HasLikelihood
::
value
>
());
return
true
;
}
}
// 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
)
{
rapidjson
::
Value
progressJson
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
std
::
uint64_t
(
p
.
progress
().
size
());
++
i
){
progressJson
.
PushBack
(
rapidjson
::
Value
().
SetInt
(
std
::
int32_t
(
p
.
progress
()[
i
])),
allocator
);
}
value
.
AddMember
(
"progress"
,
progressJson
,
allocator
);
return
true
;
}
template
<
class
ProgressType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
ProgressType
&
p
)
{
if
(
!
value
.
HasMember
(
"progress"
)
||
!
value
[
"progress"
].
IsArray
()){
assert
(
false
);
return
false
;
}
const
auto
&
jsonProgress
=
value
[
"progress"
];
unsigned
long
sz
=
jsonProgress
.
Size
();
p
.
progress
().
clear
();
p
.
progress
().
reserve
(
sz
);
for
(
unsigned
long
i
=
0
;
i
<
sz
;
++
i
)
p
.
progress
().
push_back
(
std
::
int32_t
(
jsonProgress
[
i
].
GetInt
()));
return
true
;
}
}
// namespace Progress
//! @brief Namespace containing methodes for nemo_msgs/Heartbeat generation.
namespace
Heartbeat
{
template
<
class
HeartbeatType
>
bool
toJson
(
const
HeartbeatType
&
heartbeat
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"status"
,
std
::
int32_t
(
heartbeat
.
status
()),
allocator
);
return
true
;
}
template
<
class
HeartbeatType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
HeartbeatType
&
heartbeat
)
{
if
(
!
value
.
HasMember
(
"status"
)
||
!
value
[
"status"
].
IsInt
()){
assert
(
false
);
return
false
;
}
heartbeat
.
setStatus
(
value
[
"status"
].
GetInt
());
return
true
;
}
}
// namespace Heartbeat
}
// namespace NemoMsgs
}
// namespace JsonMethodes
}
// namespace ROSBridge
#endif // MESSAGES_H
src/comm/ros_bridge/include/MessageBaseClass.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#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 MessageGroups). The Group type
//! 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.
class
MessageBaseClass
{
public:
typedef
MessageGroups
::
EmptyGroup
Group
;
MessageBaseClass
()
{};
virtual
~
MessageBaseClass
()
{};
// Avoid sliceing (copy with ROSMessage::Clone or define subclass operator=)!
MessageBaseClass
(
const
MessageBaseClass
&
)
=
delete
;
MessageBaseClass
&
operator
=
(
const
MessageBaseClass
&
)
=
delete
;
virtual
MessageBaseClass
*
Clone
()
const
=
0
;
};
typedef
MessageBaseClass
MsgBase
;
}
// namespace ROSBridge
src/comm/ros_bridge/include/MessageGroups.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#include <string>
namespace
ROSBridge
{
namespace
MessageGroups
{
typedef
std
::
string
StringType
;
template
<
typename
Group
,
typename
SubGroup
,
typename
...
MoreSubGroups
>
struct
MessageGroup
{
static
StringType
messageType
()
{
return
_full
<
Group
,
SubGroup
,
MoreSubGroups
...
>
();}
template
<
typename
G
,
typename
SubG
,
typename
...
MoreSubG
>
static
StringType
_full
()
{
return
G
::
label
()
+
"/"
+
_full
<
SubG
,
MoreSubG
...
>
();
}
template
<
typename
G
>
static
StringType
_full
()
{
return
G
::
label
();
}
static
StringType
messageTypeLast
()
{
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
();
}
};
//!
//! \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
GeographicMsgs
{
static
StringType
label
()
{
return
"geographic_msgs"
;}
//!
//! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
struct
GeoPointGroup
{
static
StringType
label
()
{
return
"GeoPoint"
;}
};
};
struct
JSKRecognitionMsgs
{
static
StringType
label
()
{
return
"jsk_recognition_msgs"
;}
//!
//! \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 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.
struct
ProgressGroup
{
static
StringType
label
()
{
return
"Progress"
;}
};
//!
//! \brief The HeartbeatGroup struct is used the mark a class as a ROS nemo_msgs/Heartbeat message.
struct
HeartbeatGroup
{
static
StringType
label
()
{
return
"Heartbeat"
;}
};
};
typedef
MessageGroup
<
detail
::
EmptyGroup
,
detail
::
EmptyGroup
>
EmptyGroup
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
GeometryMsgs
,
MessageGroups
::
GeometryMsgs
::
Point32Group
>
Point32Group
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
GeographicMsgs
,
MessageGroups
::
GeographicMsgs
::
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
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
NemoMsgs
,
MessageGroups
::
NemoMsgs
::
ProgressGroup
>
ProgressGroup
;
typedef
MessageGroups
::
MessageGroup
<
MessageGroups
::
NemoMsgs
,
MessageGroups
::
NemoMsgs
::
HeartbeatGroup
>
HeartbeatGroup
;
}
// end namespace MessageGroups
}
// end namespace ros_bridge
src/comm/ros_bridge/include/MessageTag.cpp
deleted
100644 → 0
View file @
9a7d98f8
#include "MessageTag.h"
MessageTag
::
MessageTag
()
{
}
MessageTag
::
MessageTag
(
const
std
::
string
&
topic
,
const
std
::
string
&
messageType
)
:
_topic
(
topic
)
,
_messagType
(
messageType
)
{
}
const
std
::
string
&
MessageTag
::
topic
()
const
{
return
_topic
;
}
const
std
::
string
&
MessageTag
::
messageType
()
const
{
return
_messagType
;
}
std
::
string
&
MessageTag
::
topic
()
{
return
_topic
;
}
std
::
string
&
MessageTag
::
messageType
()
{
return
_messagType
;
}
void
MessageTag
::
setTopic
(
const
std
::
string
&
topic
)
{
_topic
=
topic
;
}
void
MessageTag
::
setMessageType
(
const
std
::
string
&
messageType
)
{
_messagType
=
messageType
;
}
src/comm/ros_bridge/include/MessageTag.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#include <string>
class
MessageTag
{
public:
MessageTag
();
MessageTag
(
const
std
::
string
&
topic
,
const
std
::
string
&
messageType
);
const
std
::
string
&
topic
()
const
;
const
std
::
string
&
messageType
()
const
;
std
::
string
&
topic
();
std
::
string
&
messageType
();
void
setTopic
(
const
std
::
string
&
topic
);
void
setMessageType
(
const
std
::
string
&
messageType
);
private:
std
::
string
_topic
;
std
::
string
_messagType
;
};
typedef
MessageTag
Tag
;
src/comm/ros_bridge/include/RosBridgeClient.cpp
View file @
32d51139
...
@@ -18,18 +18,18 @@ struct Task{
...
@@ -18,18 +18,18 @@ struct Task{
void
RosbridgeWsClient
::
start
(
const
std
::
__cxx11
::
string
&
client_name
,
std
::
shared_ptr
<
WsClient
>
client
,
const
std
::
__cxx11
::
string
&
message
)
void
RosbridgeWsClient
::
start
(
const
std
::
__cxx11
::
string
&
client_name
,
std
::
shared_ptr
<
WsClient
>
client
,
const
std
::
__cxx11
::
string
&
message
)
{
{
#ifndef DEBUG
#ifndef
ROS_BRIDGE_
DEBUG
(
void
)
client_name
;
(
void
)
client_name
;
#endif
#endif
if
(
!
client
->
on_open
)
if
(
!
client
->
on_open
)
{
{
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
#endif
...
@@ -37,7 +37,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
...
@@ -37,7 +37,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
};
};
}
}
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
if
(
!
client
->
on_message
)
if
(
!
client
->
on_message
)
{
{
client
->
on_message
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
client
->
on_message
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
...
@@ -61,21 +61,21 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
...
@@ -61,21 +61,21 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
}
}
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
thread
client_thread
([
client_name
,
client
]()
{
std
::
thread
client_thread
([
client_name
,
client
]()
{
#else
#else
std
::
thread
client_thread
([
client
]()
{
std
::
thread
client_thread
([
client
]()
{
#endif
#endif
client
->
start
();
client
->
start
();
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Terminated"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Terminated"
<<
std
::
endl
;
#endif
#endif
client
->
on_open
=
NULL
;
client
->
on_open
=
NULL
;
client
->
on_message
=
NULL
;
client
->
on_message
=
NULL
;
client
->
on_close
=
NULL
;
client
->
on_close
=
NULL
;
client
->
on_error
=
NULL
;
client
->
on_error
=
NULL
;
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
" thread end"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
" thread end"
<<
std
::
endl
;
#endif
#endif
});
});
...
@@ -122,10 +122,6 @@ void RosbridgeWsClient::run()
...
@@ -122,10 +122,6 @@ void RosbridgeWsClient::run()
// ====================================================================================
// ====================================================================================
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
)
{
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
)
{
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
#ifdef DEBUG
std
::
cout
<<
"Starting new poll."
<<
std
::
endl
;
std
::
cout
<<
"connected: "
<<
this
->
isConnected
->
load
()
<<
std
::
endl
;
#endif
std
::
string
reset_status_task_name
=
"reset_status_task"
;
std
::
string
reset_status_task_name
=
"reset_status_task"
;
// Add status task if necessary.
// Add status task if necessary.
auto
const
it
=
std
::
find_if
(
task_list
.
begin
(),
task_list
.
end
(),
auto
const
it
=
std
::
find_if
(
task_list
.
begin
(),
task_list
.
end
(),
...
@@ -133,16 +129,10 @@ void RosbridgeWsClient::run()
...
@@ -133,16 +129,10 @@ void RosbridgeWsClient::run()
return
t
.
name
==
reset_status_task_name
;
return
t
.
name
==
reset_status_task_name
;
});
});
if
(
it
==
task_list
.
end
()
){
if
(
it
==
task_list
.
end
()
){
#ifdef DEBUG
std
::
cout
<<
"Adding status_task"
<<
std
::
endl
;
#endif
// Check connection status.
// Check connection status.
auto
status_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
auto
status_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
auto
status_client
=
std
::
make_shared
<
WsClient
>
(
this
->
server_port_path
);
auto
status_client
=
std
::
make_shared
<
WsClient
>
(
this
->
server_port_path
);
status_client
->
on_open
=
[
status_set
,
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
)
{
status_client
->
on_open
=
[
status_set
,
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
)
{
#ifdef DEBUG
std
::
cout
<<
"status_client opened"
<<
std
::
endl
;
#endif
this
->
isConnected
->
store
(
true
);
this
->
isConnected
->
store
(
true
);
status_set
->
store
(
true
);
status_set
->
store
(
true
);
};
};
...
@@ -190,16 +180,10 @@ void RosbridgeWsClient::run()
...
@@ -190,16 +180,10 @@ void RosbridgeWsClient::run()
});
});
if
(
topics_it
==
task_list
.
end
()
){
if
(
topics_it
==
task_list
.
end
()
){
// Call /rosapi/topics service.
// Call /rosapi/topics service.
#ifdef DEBUG
std
::
cout
<<
"Adding reset_topics_task"
<<
std
::
endl
;
#endif
auto
topics_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
auto
topics_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
this
->
callService
(
"/rosapi/topics"
,
[
topics_set
,
this
](
this
->
callService
(
"/rosapi/topics"
,
[
topics_set
,
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
#ifdef DEBUG
std
::
cout
<<
"/rosapi/topics: "
<<
in_message
->
string
()
<<
std
::
endl
;
#endif
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_topics
=
in_message
->
string
();
this
->
available_topics
=
in_message
->
string
();
lk
.
unlock
();
lk
.
unlock
();
...
@@ -241,16 +225,10 @@ void RosbridgeWsClient::run()
...
@@ -241,16 +225,10 @@ void RosbridgeWsClient::run()
});
});
if
(
services_it
==
task_list
.
end
()
){
if
(
services_it
==
task_list
.
end
()
){
// Call /rosapi/services service.
// Call /rosapi/services service.
#ifdef DEBUG
std
::
cout
<<
"Adding reset_services_task"
<<
std
::
endl
;
#endif
auto
services_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
auto
services_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
this
->
callService
(
"/rosapi/services"
,
[
this
,
services_set
](
this
->
callService
(
"/rosapi/services"
,
[
this
,
services_set
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
#ifdef DEBUG
std
::
cout
<<
"/rosapi/services: "
<<
in_message
->
string
()
<<
std
::
endl
;
#endif
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_services
=
in_message
->
string
();
this
->
available_services
=
in_message
->
string
();
lk
.
unlock
();
lk
.
unlock
();
...
@@ -295,26 +273,14 @@ void RosbridgeWsClient::run()
...
@@ -295,26 +273,14 @@ void RosbridgeWsClient::run()
// Process tasks.
// Process tasks.
// ====================================================================================
// ====================================================================================
for
(
auto
task_it
=
task_list
.
begin
();
task_it
!=
task_list
.
end
();
){
for
(
auto
task_it
=
task_list
.
begin
();
task_it
!=
task_list
.
end
();
){
#ifdef DEBUG
std
::
cout
<<
"processing task: "
<<
task_it
->
name
<<
std
::
endl
;
#endif
if
(
!
task_it
->
expired
()
){
if
(
!
task_it
->
expired
()
){
if
(
task_it
->
ready
()
){
if
(
task_it
->
ready
()
){
#ifdef DEBUG
std
::
cout
<<
"executing task: "
<<
task_it
->
name
<<
std
::
endl
;
#endif
task_it
->
execute
();
task_it
->
execute
();
task_it
=
task_list
.
erase
(
task_it
);
task_it
=
task_list
.
erase
(
task_it
);
}
else
{
}
else
{
#ifdef DEBUG
std
::
cout
<<
"noting to do for task: "
<<
task_it
->
name
<<
std
::
endl
;
#endif
++
task_it
;
++
task_it
;
}
}
}
else
{
}
else
{
#ifdef DEBUG
std
::
cout
<<
"task expired: "
<<
task_it
->
name
<<
std
::
endl
;
#endif
task_it
->
clear_up
();
task_it
->
clear_up
();
task_it
=
task_list
.
erase
(
task_it
);
task_it
=
task_list
.
erase
(
task_it
);
}
}
...
@@ -328,7 +294,7 @@ void RosbridgeWsClient::run()
...
@@ -328,7 +294,7 @@ void RosbridgeWsClient::run()
task_it
->
clear_up
();
task_it
->
clear_up
();
}
}
task_list
.
clear
();
task_list
.
clear
();
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
"periodic thread end"
<<
std
::
endl
;
std
::
cout
<<
"periodic thread end"
<<
std
::
endl
;
#endif
#endif
});
});
...
@@ -349,9 +315,14 @@ void RosbridgeWsClient::reset()
...
@@ -349,9 +315,14 @@ void RosbridgeWsClient::reset()
unsubscribeAll
();
unsubscribeAll
();
unadvertiseAll
();
unadvertiseAll
();
unadvertiseAllServices
();
unadvertiseAllServices
();
for
(
auto
&
client
:
client_map
)
{
std
::
unique_lock
<
std
::
mutex
>
lk
(
mutex
);
removeClient
(
client
.
first
);
for
(
auto
it
=
client_map
.
begin
();
it
!=
client_map
.
end
();
)
{
std
::
string
client_name
=
it
->
first
;
lk
.
unlock
();
removeClient
(
client_name
);
lk
.
lock
();
it
=
client_map
.
begin
();
}
}
}
}
...
@@ -363,7 +334,7 @@ void RosbridgeWsClient::addClient(const std::string &client_name)
...
@@ -363,7 +334,7 @@ void RosbridgeWsClient::addClient(const std::string &client_name)
{
{
client_map
[
client_name
]
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
client_map
[
client_name
]
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
}
}
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
else
else
{
{
std
::
cerr
<<
client_name
<<
" has already been created"
<<
std
::
endl
;
std
::
cerr
<<
client_name
<<
" has already been created"
<<
std
::
endl
;
...
@@ -391,7 +362,7 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
...
@@ -391,7 +362,7 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
// Stop the client asynchronously in 100 ms.
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving the client have been launched.
// This is to ensure, that all threads involving the client have been launched.
std
::
shared_ptr
<
WsClient
>
client
=
it
->
second
;
std
::
shared_ptr
<
WsClient
>
client
=
it
->
second
;
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
thread
t
([
client
,
client_name
](){
std
::
thread
t
([
client
,
client_name
](){
#else
#else
std
::
thread
t
([
client
](){
std
::
thread
t
([
client
](){
...
@@ -403,14 +374,13 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
...
@@ -403,14 +374,13 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
// client->on_message = NULL;
// client->on_message = NULL;
// client->on_close = NULL;
// client->on_close = NULL;
// client->on_error = NULL;
// client->on_error = NULL;
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
"removeClient thread: "
<<
client_name
<<
" reference count: "
<<
client
.
use_count
()
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
" has been removed"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
" has been removed"
<<
std
::
endl
;
#endif
#endif
});
});
t
.
detach
();
t
.
detach
();
}
}
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
else
else
{
{
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
...
@@ -428,7 +398,7 @@ void RosbridgeWsClient::removeClient(const std::string &client_name)
...
@@ -428,7 +398,7 @@ void RosbridgeWsClient::removeClient(const std::string &client_name)
{
{
client_map
.
erase
(
it
);
client_map
.
erase
(
it
);
}
}
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
else
else
{
{
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
...
@@ -448,15 +418,24 @@ std::string RosbridgeWsClient::getAdvertisedServices(){
...
@@ -448,15 +418,24 @@ std::string RosbridgeWsClient::getAdvertisedServices(){
}
}
bool
RosbridgeWsClient
::
topicAvailable
(
const
std
::
string
&
topic
){
bool
RosbridgeWsClient
::
topicAvailable
(
const
std
::
string
&
topic
){
#ifdef DEBUG
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
"checking if topic "
<<
topic
<<
" is available"
<<
std
::
endl
;
std
::
cout
<<
"checking if topic "
<<
topic
<<
" is available"
<<
std
::
endl
;
std
::
cout
<<
"available topics: "
<<
available_topics
<<
std
::
endl
;
#endif
#endif
size_t
pos
;
size_t
pos
;
{
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
pos
=
available_topics
.
find
(
topic
);
pos
=
available_topics
.
find
(
topic
);
}
}
return
pos
!=
std
::
string
::
npos
?
true
:
false
;
bool
ret
=
pos
!=
std
::
string
::
npos
?
true
:
false
;
#ifdef ROS_BRIDGE_DEBUG
if
(
ret
){
std
::
cout
<<
"topic "
<<
topic
<<
" is available"
<<
std
::
endl
;
}
else
{
std
::
cout
<<
"topic "
<<
topic
<<
" is not available"
<<
std
::
endl
;
}
#endif
return
ret
;
}
}
void
RosbridgeWsClient
::
advertise
(
const
std
::
string
&
client_name
,
const
std
::
string
&
topic
,
const
std
::
string
&
type
,
const
std
::
string
&
id
)
void
RosbridgeWsClient
::
advertise
(
const
std
::
string
&
client_name
,
const
std
::
string
&
topic
,
const
std
::
string
&
type
,
const
std
::
string
&
id
)
...
@@ -471,7 +450,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
...
@@ -471,7 +450,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
!=
service_topic_list
.
end
()){
if
(
it_ser_top
!=
service_topic_list
.
end
()){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
std
::
cerr
<<
"topic: "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
#endif
#endif
return
;
return
;
...
@@ -487,13 +466,13 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
...
@@ -487,13 +466,13 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
}
}
message
=
"{"
+
message
+
"}"
;
message
=
"{"
+
message
+
"}"
;
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
client
->
on_open
=
[
this
,
topic
,
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
this
,
topic
,
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
#else
client
->
on_open
=
[
this
,
topic
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
this
,
topic
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
#endif
...
@@ -502,7 +481,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
...
@@ -502,7 +481,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
start
(
client_name
,
client
,
message
);
start
(
client_name
,
client
,
message
);
}
}
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
else
else
{
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
...
@@ -518,7 +497,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
...
@@ -518,7 +497,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
==
service_topic_list
.
end
()){
if
(
it_ser_top
==
service_topic_list
.
end
()){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" not advertised"
<<
std
::
endl
;
std
::
cerr
<<
"topic: "
<<
topic
<<
" not advertised"
<<
std
::
endl
;
#endif
#endif
return
;
return
;
...
@@ -534,13 +513,13 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
...
@@ -534,13 +513,13 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
std
::
string
client_name
=
"topic_unadvertiser"
+
topic
;
std
::
string
client_name
=
"topic_unadvertiser"
+
topic
;
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
#endif
...
@@ -569,7 +548,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
...
@@ -569,7 +548,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
==
service_topic_list
.
end
()
){
if
(
it_ser_top
==
service_topic_list
.
end
()
){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" not yet advertised"
<<
std
::
endl
;
std
::
cerr
<<
"topic: "
<<
topic
<<
" not yet advertised"
<<
std
::
endl
;
#endif
#endif
return
;
return
;
...
@@ -588,12 +567,12 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
...
@@ -588,12 +567,12 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
message
=
"{"
+
message
+
"}"
;
message
=
"{"
+
message
+
"}"
;
std
::
shared_ptr
<
WsClient
>
publish_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
std
::
shared_ptr
<
WsClient
>
publish_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
publish_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
publish_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
#else
publish_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
publish_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message."
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message."
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
...
@@ -619,7 +598,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
...
@@ -619,7 +598,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
!=
service_topic_list
.
end
()){
if
(
it_ser_top
!=
service_topic_list
.
end
()){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
std
::
cerr
<<
"topic: "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
#endif
#endif
return
;
return
;
...
@@ -659,7 +638,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
...
@@ -659,7 +638,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
client
->
on_message
=
callback
;
client
->
on_message
=
callback
;
this
->
start
(
client_name
,
client
,
message
);
// subscribe to topic
this
->
start
(
client_name
,
client
,
message
);
// subscribe to topic
}
}
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
else
else
{
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
...
@@ -675,7 +654,7 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
...
@@ -675,7 +654,7 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
==
service_topic_list
.
end
()){
if
(
it_ser_top
==
service_topic_list
.
end
()){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" not advertised"
<<
std
::
endl
;
std
::
cerr
<<
"topic: "
<<
topic
<<
" not advertised"
<<
std
::
endl
;
#endif
#endif
return
;
return
;
...
@@ -691,13 +670,13 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
...
@@ -691,13 +670,13 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
std
::
string
client_name
=
"topic_unsubscriber"
+
topic
;
std
::
string
client_name
=
"topic_unsubscriber"
+
topic
;
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
#endif
...
@@ -729,7 +708,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
...
@@ -729,7 +708,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
return
service
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
service
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
!=
service_topic_list
.
end
()){
if
(
it_ser_top
!=
service_topic_list
.
end
()){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cerr
<<
"service: "
<<
service
<<
" already advertised"
<<
std
::
endl
;
std
::
cerr
<<
"service: "
<<
service
<<
" already advertised"
<<
std
::
endl
;
#endif
#endif
return
;
return
;
...
@@ -743,7 +722,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
...
@@ -743,7 +722,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
it_client
->
second
->
on_message
=
callback
;
it_client
->
second
->
on_message
=
callback
;
start
(
client_name
,
it_client
->
second
,
message
);
start
(
client_name
,
it_client
->
second
,
message
);
}
}
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
else
else
{
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
...
@@ -759,7 +738,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
...
@@ -759,7 +738,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
return
service
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
service
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
==
service_topic_list
.
end
()){
if
(
it_ser_top
==
service_topic_list
.
end
()){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cerr
<<
"service: "
<<
service
<<
" not advertised"
<<
std
::
endl
;
std
::
cerr
<<
"service: "
<<
service
<<
" not advertised"
<<
std
::
endl
;
#endif
#endif
return
;
return
;
...
@@ -771,13 +750,13 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
...
@@ -771,13 +750,13 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
std
::
string
client_name
=
"service_unadvertiser"
+
service
;
std
::
string
client_name
=
"service_unadvertiser"
+
service
;
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
#endif
...
@@ -816,12 +795,12 @@ void RosbridgeWsClient::serviceResponse(const std::string &service, const std::s
...
@@ -816,12 +795,12 @@ void RosbridgeWsClient::serviceResponse(const std::string &service, const std::s
std
::
string
client_name
=
"service_response_client"
+
service
;
std
::
string
client_name
=
"service_response_client"
+
service
;
std
::
shared_ptr
<
WsClient
>
service_response_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
std
::
shared_ptr
<
WsClient
>
service_response_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
service_response_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
service_response_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
#else
service_response_client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
service_response_client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#endif
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
#endif
...
@@ -869,7 +848,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
...
@@ -869,7 +848,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
else
else
{
{
call_service_client
->
on_message
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
call_service_client
->
on_message
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
client_name
<<
": Message received: "
<<
in_message
->
string
()
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Message received: "
<<
in_message
->
string
()
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending close connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending close connection"
<<
std
::
endl
;
#else
#else
...
@@ -884,7 +863,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
...
@@ -884,7 +863,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
bool
RosbridgeWsClient
::
serviceAvailable
(
const
std
::
string
&
service
)
bool
RosbridgeWsClient
::
serviceAvailable
(
const
std
::
string
&
service
)
{
{
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
std
::
cout
<<
"checking if service "
<<
service
<<
" is available"
<<
std
::
endl
;
std
::
cout
<<
"checking if service "
<<
service
<<
" is available"
<<
std
::
endl
;
#endif
#endif
size_t
pos
;
size_t
pos
;
...
@@ -892,7 +871,16 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
...
@@ -892,7 +871,16 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
pos
=
available_services
.
find
(
service
);
pos
=
available_services
.
find
(
service
);
}
}
return
pos
!=
std
::
string
::
npos
?
true
:
false
;
bool
ret
=
pos
!=
std
::
string
::
npos
?
true
:
false
;
#ifdef ROS_BRIDGE_DEBUG
if
(
ret
){
std
::
cout
<<
"service "
<<
service
<<
" is available"
<<
std
::
endl
;
}
else
{
std
::
cout
<<
"service "
<<
service
<<
" is not available"
<<
std
::
endl
;
}
#endif
return
ret
;
}
}
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
)
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
)
...
@@ -904,28 +892,27 @@ void RosbridgeWsClient::waitForService(const std::string &service)
...
@@ -904,28 +892,27 @@ void RosbridgeWsClient::waitForService(const std::string &service)
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
,
const
std
::
function
<
bool
(
void
)
>
stop
)
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
,
const
std
::
function
<
bool
(
void
)
>
stop
)
{
{
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
long
counter
=
0
;
long
counter
=
0
;
#endif
#endif
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
();
while
(
!
stop
()
)
while
(
!
stop
()
)
{
{
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
++
counter
;
++
counter
;
#endif
#endif
if
(
serviceAvailable
(
service
)
){
if
(
serviceAvailable
(
service
)
){
break
;
break
;
}
else
{
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
}
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
else
{
}
else
{
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
}
}
};
};
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
auto
e
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
e
=
std
::
chrono
::
high_resolution_clock
::
now
();
std
::
cout
<<
"waitForService() "
<<
service
<<
" time: "
std
::
cout
<<
"waitForService() "
<<
service
<<
" time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
e
-
s
).
count
()
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
e
-
s
).
count
()
...
@@ -944,28 +931,27 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic)
...
@@ -944,28 +931,27 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic)
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
function
<
bool
(
void
)
>
stop
)
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
function
<
bool
(
void
)
>
stop
)
{
{
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
long
counter
=
0
;
long
counter
=
0
;
#endif
#endif
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
();
while
(
!
stop
()
)
while
(
!
stop
()
)
{
{
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
++
counter
;
++
counter
;
#endif
#endif
if
(
topicAvailable
(
topic
)
){
if
(
topicAvailable
(
topic
)
){
break
;
break
;
}
else
{
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
}
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
else
{
}
else
{
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
}
}
};
};
#ifdef DEBUG
#ifdef
ROS_BRIDGE_
DEBUG
auto
e
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
e
=
std
::
chrono
::
high_resolution_clock
::
now
();
std
::
cout
<<
"waitForTopic() "
<<
topic
<<
" time: "
std
::
cout
<<
"waitForTopic() "
<<
topic
<<
" time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
e
-
s
).
count
()
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
e
-
s
).
count
()
...
...
src/comm/ros_bridge/include/ThreadSafeQueue.h
deleted
100644 → 0
View file @
9a7d98f8
#pragma once
#include <queue>
#include <mutex>
#include <condition_variable>
namespace
ROSBridge
{
template
<
class
T
>
class
ThreadSafeQueue
{
public:
ThreadSafeQueue
();
~
ThreadSafeQueue
();
T
pop_front
();
void
push_back
(
const
T
&
item
);
void
push_back
(
T
&&
item
);
int
size
();
bool
empty
();
private:
std
::
deque
<
T
>
_queue
;
std
::
mutex
_mutex
;
std
::
condition_variable
_cond
;
};
template
<
typename
T
>
ThreadSafeQueue
<
T
>::
ThreadSafeQueue
(){}
template
<
typename
T
>
ThreadSafeQueue
<
T
>::~
ThreadSafeQueue
(){}
template
<
typename
T
>
T
ThreadSafeQueue
<
T
>::
pop_front
()
{
std
::
unique_lock
<
std
::
mutex
>
mlock
(
_mutex
);
while
(
_queue
.
empty
())
{
_cond
.
wait
(
mlock
);
}
T
t
=
std
::
move
(
_queue
.
front
());
_queue
.
pop_front
();
return
t
;
}
template
<
typename
T
>
void
ThreadSafeQueue
<
T
>::
push_back
(
const
T
&
item
)
{
std
::
unique_lock
<
std
::
mutex
>
mlock
(
_mutex
);
_queue
.
push_back
(
item
);
mlock
.
unlock
();
// unlock before notificiation to minimize mutex con
_cond
.
notify_one
();
// notify one waiting thread
}
template
<
typename
T
>
void
ThreadSafeQueue
<
T
>::
push_back
(
T
&&
item
)
{
std
::
unique_lock
<
std
::
mutex
>
mlock
(
_mutex
);
_queue
.
push_back
(
std
::
move
(
item
));
mlock
.
unlock
();
// unlock before notificiation to minimize mutex con
_cond
.
notify_one
();
// notify one waiting thread
}
template
<
typename
T
>
int
ThreadSafeQueue
<
T
>::
size
()
{
std
::
unique_lock
<
std
::
mutex
>
mlock
(
_mutex
);
int
size
=
_queue
.
size
();
mlock
.
unlock
();
return
size
;
}
}
// namespace
src/comm/ros_bridge/include/TopicPublisher.cpp
deleted
100644 → 0
View file @
9a7d98f8
#include "TopicPublisher.h"
#include <unordered_map>
ROSBridge
::
ComPrivate
::
TopicPublisher
::
TopicPublisher
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
)
:
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_casePacker
(
casePacker
)
,
_rbc
(
rbc
)
{
}
ROSBridge
::
ComPrivate
::
TopicPublisher
::~
TopicPublisher
()
{
this
->
reset
();
}
void
ROSBridge
::
ComPrivate
::
TopicPublisher
::
start
()
{
if
(
!
_stopped
->
load
()
)
// start called while thread running.
return
;
_stopped
->
store
(
false
);
_pThread
=
std
::
make_unique
<
std
::
thread
>
([
this
]{
// Init.
std
::
unordered_map
<
std
::
string
,
std
::
string
>
topicMap
;
// Main Loop.
while
(
!
this
->
_stopped
->
load
()
){
JsonDocUPtr
pJsonDoc
;
{
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
// Check if new data available, wait if not.
if
(
this
->
_queue
.
empty
()){
if
(
_stopped
->
load
()
)
// Check condition again while holding the lock.
break
;
this
->
_cv
.
wait
(
lk
);
// Wait for condition, spurious wakeups don't matter in this case.
continue
;
}
// Pop message from queue.
pJsonDoc
=
std
::
move
(
this
->
_queue
.
front
());
this
->
_queue
.
pop_front
();
}
// Get tag from Json message and remove it.
Tag
tag
;
bool
ret
=
this
->
_casePacker
.
getTag
(
pJsonDoc
,
tag
);
assert
(
ret
);
// Json message does not contain a tag;
(
void
)
ret
;
this
->
_casePacker
.
removeTag
(
pJsonDoc
);
// Check if topic must be advertised.
// Advertised topics are stored in advertisedTopicsHashList as
// a hash.
std
::
string
clientName
=
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
+
tag
.
topic
();
auto
it
=
topicMap
.
find
(
clientName
);
if
(
it
==
topicMap
.
end
())
{
// Need to advertise topic?
topicMap
.
insert
(
std
::
make_pair
(
clientName
,
tag
.
topic
()));
this
->
_rbc
.
addClient
(
clientName
);
this
->
_rbc
.
advertise
(
clientName
,
tag
.
topic
(),
tag
.
messageType
()
);
this
->
_rbc
.
waitForTopic
(
tag
.
topic
(),
[
this
]{
return
this
->
_stopped
->
load
();
});
// Wait until topic is advertised.
}
// Publish Json message.
if
(
!
this
->
_stopped
->
load
()
)
this
->
_rbc
.
publish
(
tag
.
topic
(),
*
pJsonDoc
.
get
());
}
// while loop
// Tidy up.
for
(
auto
&
it
:
topicMap
){
this
->
_rbc
.
unadvertise
(
it
.
second
);
this
->
_rbc
.
removeClient
(
it
.
first
);
}
std
::
cout
<<
"TopicPublisher: publisher thread terminated."
<<
std
::
endl
;
});
}
void
ROSBridge
::
ComPrivate
::
TopicPublisher
::
reset
()
{
if
(
_stopped
->
load
()
)
// stop called while thread not running.
return
;
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
_mutex
);
std
::
cout
<<
"TopicPublisher: _stopped->store(true)."
<<
std
::
endl
;
_stopped
->
store
(
true
);
std
::
cout
<<
"TopicPublisher: ask publisher thread to stop."
<<
std
::
endl
;
_cv
.
notify_one
();
// Wake publisher thread.
}
if
(
!
_pThread
)
return
;
_pThread
->
join
();
std
::
cout
<<
"TopicPublisher: publisher thread joined."
<<
std
::
endl
;
{
_queue
.
clear
();
std
::
cout
<<
"TopicPublisher: queue cleard."
<<
std
::
endl
;
}
}
src/comm/ros_bridge/include/TypeFactory.h
deleted
100644 → 0
View file @
9a7d98f8
#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.
class
TypeFactory
{
typedef
MessageBaseClass
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!
return
false
;
}
// ===========================================================================
// Point32Group
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
Point32Group
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
GeometryMsgs
::
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
::
GeographicMsgs
::
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
::
GeometryMsgs
::
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
::
GeometryMsgs
::
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
::
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
;
}
// ===========================================================================
// HeartbeatGroup
// ===========================================================================
template
<
class
U
>
bool
_create
(
const
rapidjson
::
Document
&
doc
,
U
&
type
,
Type2Type
<
MessageGroups
::
HeartbeatGroup
>
){
using
namespace
ROSBridge
;
bool
ret
=
JsonMethodes
::
NemoMsgs
::
Heartbeat
::
fromJson
(
doc
,
type
);
assert
(
ret
);
return
ret
;
}
};
}
// end namespace ros_bridge
src/comm/ros_bridge/include/com_private.cpp
0 → 100644
View file @
32d51139
#include "ros_bridge/include/com_private.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include <functional>
#include <iostream>
std
::
size_t
ros_bridge
::
com_private
::
getHash
(
const
std
::
string
&
str
)
{
std
::
hash
<
std
::
string
>
hash
;
return
hash
(
str
);
}
std
::
size_t
ros_bridge
::
com_private
::
getHash
(
const
char
*
str
)
{
return
ros_bridge
::
com_private
::
getHash
(
std
::
string
(
str
));
}
bool
ros_bridge
::
com_private
::
getTopic
(
const
ros_bridge
::
com_private
::
JsonDoc
&
doc
,
std
::
string
&
topic
)
{
if
(
doc
.
HasMember
(
"topic"
)
&&
doc
[
"topic"
].
IsString
()
){
topic
=
doc
[
"topic"
].
GetString
();
return
true
;
}
else
return
false
;
}
bool
ros_bridge
::
com_private
::
removeTopic
(
ros_bridge
::
com_private
::
JsonDoc
&
doc
)
{
if
(
doc
.
HasMember
(
"topic"
)
&&
doc
[
"topic"
].
IsString
()
){
doc
.
RemoveMember
(
"topic"
);
return
true
;
}
else
return
false
;
}
bool
ros_bridge
::
com_private
::
getType
(
const
ros_bridge
::
com_private
::
JsonDoc
&
doc
,
std
::
string
&
type
)
{
if
(
doc
.
HasMember
(
"type"
)
&&
doc
[
"type"
].
IsString
()
){
type
=
doc
[
"type"
].
GetString
();
return
true
;
}
else
return
false
;
}
bool
ros_bridge
::
com_private
::
removeType
(
ros_bridge
::
com_private
::
JsonDoc
&
doc
)
{
if
(
doc
.
HasMember
(
"type"
)
&&
doc
[
"type"
].
IsString
()
){
doc
.
RemoveMember
(
"type"
);
return
true
;
}
else
return
false
;
}
src/comm/ros_bridge/include/
ComPrivateInclud
e.h
→
src/comm/ros_bridge/include/
com_privat
e.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/MessageTag.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <memory>
#include <memory>
#include <deque>
#include <deque>
#include <unordered_map>
#include <unordered_map>
namespace
ROSB
ridge
{
namespace
ros_b
ridge
{
namespace
ComP
rivate
{
namespace
com_p
rivate
{
typedef
MessageTag
Tag
;
typedef
rapidjson
::
Document
JsonDoc
;
typedef
rapidjson
::
Document
JsonDoc
;
typedef
std
::
unique_ptr
<
JsonDoc
>
JsonDocUPtr
;
typedef
std
::
unique_ptr
<
JsonDoc
>
JsonDocUPtr
;
typedef
std
::
deque
<
JsonDocUPtr
>
JsonQueue
;
typedef
std
::
deque
<
JsonDocUPtr
>
JsonQueue
;
...
@@ -26,5 +24,11 @@ static const char* _topicSubscriberKey = "topic_subscriber";
...
@@ -26,5 +24,11 @@ static const char* _topicSubscriberKey = "topic_subscriber";
std
::
size_t
getHash
(
const
std
::
string
&
str
);
std
::
size_t
getHash
(
const
std
::
string
&
str
);
std
::
size_t
getHash
(
const
char
*
str
);
std
::
size_t
getHash
(
const
char
*
str
);
bool
getTopic
(
const
JsonDoc
&
doc
,
std
::
string
&
topic
);
bool
getType
(
const
JsonDoc
&
doc
,
std
::
string
&
type
);
bool
removeTopic
(
JsonDoc
&
doc
);
bool
removeType
(
JsonDoc
&
doc
);
}
}
}
}
src/comm/ros_bridge/include/
MessageT
raits.h
→
src/comm/ros_bridge/include/
message_t
raits.h
View file @
32d51139
#pragma once
#pragma once
namespace
ROSB
ridge
{
namespace
ros_b
ridge
{
namespace
traits
{
namespace
traits
{
...
@@ -182,5 +182,19 @@ struct Select<0, T, U> {typedef U Result;};
...
@@ -182,5 +182,19 @@ struct Select<0, T, U> {typedef U Result;};
struct
HasComponents
{};
struct
HasComponents
{};
struct
Has3Components
:
public
HasComponents
{};
struct
Has3Components
:
public
HasComponents
{};
struct
Has2Components
:
public
HasComponents
{};
struct
Has2Components
:
public
HasComponents
{};
template
<
typename
T
>
struct
Type2Type
{
typedef
T
OriginalType
;
};
template
<
int
k
>
struct
Int2Type
{
enum
{
value
=
k
};
};
}
}
}
}
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp
0 → 100644
View file @
32d51139
#include "geopoint.h"
std
::
string
ros_bridge
::
messages
::
geographic_msgs
::
geo_point
::
messageType
(){
return
"geographic_msgs/GeoPoint"
;
}
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
0 → 100644
View file @
32d51139
#pragma once
#include <string>
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
geographic_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
namespace
geo_point
{
std
::
string
messageType
();
//! @brief C++ representation of geographic_msgs/GeoPoint.
template
<
class
FloatType
=
_Float64
,
class
OStream
=
std
::
ostream
>
class
GenericGeoPoint
{
public:
GenericGeoPoint
()
:
_latitude
(
0
)
,
_longitude
(
0
)
,
_altitude
(
0
)
{}
GenericGeoPoint
(
const
GenericGeoPoint
&
other
)
:
_latitude
(
other
.
latitude
())
,
_longitude
(
other
.
longitude
())
,
_altitude
(
other
.
altitude
())
{}
GenericGeoPoint
(
FloatType
latitude
,
FloatType
longitude
,
FloatType
altitude
)
:
_latitude
(
latitude
)
,
_longitude
(
longitude
)
,
_altitude
(
altitude
)
{}
FloatType
latitude
()
const
{
return
_latitude
;}
FloatType
longitude
()
const
{
return
_longitude
;}
FloatType
altitude
()
const
{
return
_altitude
;}
void
setLatitude
(
FloatType
latitude
)
{
_latitude
=
latitude
;}
void
setLongitude
(
FloatType
longitude
)
{
_longitude
=
longitude
;}
void
setAltitude
(
FloatType
altitude
)
{
_altitude
=
altitude
;}
bool
operator
==
(
const
GenericGeoPoint
&
p1
)
{
return
(
p1
.
_latitude
==
this
->
_latitude
&&
p1
.
_longitude
==
this
->
_longitude
&&
p1
.
_altitude
==
this
->
_altitude
);
}
bool
operator
!=
(
const
GenericGeoPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericGeoPoint
&
p
)
{
os
<<
"lat: "
<<
p
.
_latitude
<<
" lon: "
<<
p
.
_longitude
<<
" alt: "
<<
p
.
_altitude
;
return
os
;
}
private:
FloatType
_latitude
;
FloatType
_longitude
;
FloatType
_altitude
;
};
typedef
GenericGeoPoint
<>
GeoPoint
;
namespace
detail
{
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
return
p
.
altitude
();
}
template
<
class
T
>
auto
getAltitude
(
const
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
}
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has3Components
>
)
{
p
.
setAltitude
(
doc
.
GetFloat
());
}
template
<
class
T
>
void
setAltitude
(
const
rapidjson
::
Value
&
doc
,
T
&
p
,
traits
::
Type2Type
<
traits
::
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
p
;
}
}
// namespace detail
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"latitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
latitude
()),
allocator
);
value
.
AddMember
(
"longitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
p
.
longitude
()),
allocator
);
typedef
typename
traits
::
Select
<
traits
::
HasMemberAltitude
<
T
>::
value
,
traits
::
Has3Components
,
traits
::
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
auto
altitude
=
detail
::
getAltitude
(
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude() replace it by 0.0;
value
.
AddMember
(
"altitude"
,
rapidjson
::
Value
().
SetFloat
((
_Float64
)
altitude
),
allocator
);
return
true
;
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
if
(
!
value
.
HasMember
(
"latitude"
)
||
!
value
[
"latitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"longitude"
)
||
!
value
[
"longitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"altitude"
)
||
!
value
[
"altitude"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setLatitude
(
value
[
"latitude"
].
GetFloat
());
p
.
setLongitude
(
value
[
"longitude"
].
GetFloat
());
typedef
typename
traits
::
Select
<
traits
::
HasMemberSetAltitude
<
PointType
>::
value
,
traits
::
Has3Components
,
traits
::
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
detail
::
setAltitude
(
value
[
"altitude"
],
p
,
traits
::
Type2Type
<
Components
>
());
// If T has no member altitude() discard doc["altitude"];
return
true
;
}
// namespace detail
}
// namespace geopoint
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp
0 → 100644
View file @
32d51139
#include "point32.h"
std
::
string
ros_bridge
::
messages
::
geometry_msgs
::
point32
::
messageType
(){
return
"geometry_msgs/Point32"
;
}
src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
geometry_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
namespace
point32
{
std
::
string
messageType
();
namespace
detail
{
using
namespace
ros_bridge
::
traits
;
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has3Components
>
)
{
return
p
.
z
();
}
template
<
class
T
>
auto
getZ
(
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
p
;
return
0
.
0
;
// p has no member z() -> add 0.
}
template
<
class
T
,
typename
V
>
bool
setZ
(
const
rapidjson
::
Value
&
doc
,
const
T
&
p
,
Type2Type
<
V
>
)
{
p
.
setZ
(
doc
[
"z"
].
GetFloat
());
return
true
;
}
template
<
class
T
>
bool
setZ
(
const
rapidjson
::
Value
&
doc
,
const
T
&
p
,
Type2Type
<
Has2Components
>
)
{
(
void
)
doc
;
(
void
)
p
;
return
true
;
}
}
// namespace detail
//! @brief C++ representation of a geometry_msgs/Point/Point32
template
<
typename
FloatType
=
_Float64
,
class
OStream
=
std
::
ostream
>
class
GenericPoint
{
public:
GenericPoint
(
FloatType
x
,
FloatType
y
,
FloatType
z
)
:
_x
(
x
),
_y
(
y
),
_z
(
z
){}
FloatType
x
()
const
{
return
_x
;}
FloatType
y
()
const
{
return
_y
;}
FloatType
z
()
const
{
return
_z
;}
void
setX
(
FloatType
x
)
{
_x
=
x
;}
void
setY
(
FloatType
y
)
{
_y
=
y
;}
void
setZ
(
FloatType
z
)
{
_z
=
z
;}
bool
operator
==
(
const
GenericPoint
&
p1
)
{
return
(
p1
.
_x
==
this
->
_x
&&
p1
.
_y
==
this
->
_y
&&
p1
.
_z
==
this
->
_z
);
}
bool
operator
!=
(
const
GenericPoint
&
p1
)
{
return
!
this
->
operator
==
(
p1
);
}
friend
OStream
&
operator
<<
(
OStream
&
os
,
const
GenericPoint
&
p
)
{
os
<<
"x: "
<<
p
.
_x
<<
" y: "
<<
p
.
_y
<<
" z: "
<<
p
.
_z
;
return
os
;
}
private:
FloatType
_x
;
FloatType
_y
;
FloatType
_z
;
};
typedef
GenericPoint
<>
Point
;
typedef
GenericPoint
<
_Float32
>
Point32
;
template
<
class
T
>
bool
toJson
(
const
T
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
ros_bridge
::
traits
;
value
.
AddMember
(
"x"
,
rapidjson
::
Value
().
SetFloat
(
p
.
x
()),
allocator
);
value
.
AddMember
(
"y"
,
rapidjson
::
Value
().
SetFloat
(
p
.
y
()),
allocator
);
typedef
typename
Select
<
HasMemberZ
<
T
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
auto
z
=
detail
::
getZ
(
p
,
Type2Type
<
Components
>
());
// If T has no member z() replace it by 0.
value
.
AddMember
(
"z"
,
rapidjson
::
Value
().
SetFloat
(
z
),
allocator
);
return
true
;
}
template
<
class
PointType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PointType
&
p
)
{
using
namespace
ros_bridge
::
traits
;
if
(
!
value
.
HasMember
(
"x"
)
||
!
value
[
"x"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"y"
)
||
!
value
[
"y"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"z"
)
||
!
value
[
"z"
].
IsFloat
()){
assert
(
false
);
return
false
;
}
p
.
setX
(
value
[
"x"
].
GetFloat
());
p
.
setY
(
value
[
"y"
].
GetFloat
());
typedef
typename
Select
<
HasMemberSetZ
<
PointType
>::
value
,
Has3Components
,
Has2Components
>::
Result
Components
;
// Check if PointType has 2 or 3 dimensions.
(
void
)
detail
::
setZ
(
value
[
"z"
],
p
,
Type2Type
<
Components
>
());
// If PointType has no member z() discard doc["z"].
return
true
;
}
}
// namespace point32
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp
0 → 100644
View file @
32d51139
#include "polygon.h"
std
::
string
ros_bridge
::
messages
::
geometry_msgs
::
polygon
::
messageType
(){
return
"geometry_msgs/Polygon"
;
}
src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/point32.h"
#include <type_traits>
#include <vector>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
geometry_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
namespace
polygon
{
std
::
string
messageType
();
//! @brief C++ representation of geometry_msgs/Polygon
template
<
class
PointTypeCVR
,
template
<
class
,
class
...
>
class
ContainerType
=
std
::
vector
>
class
GenericPolygon
{
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
PointTypeCVR
>>
PointType
;
public:
GenericPolygon
()
{}
GenericPolygon
(
const
GenericPolygon
&
poly
)
:
_points
(
poly
.
points
()){}
const
ContainerType
<
PointType
>
&
points
()
const
{
return
_points
;}
ContainerType
<
PointType
>
&
points
()
{
return
_points
;}
GenericPolygon
&
operator
=
(
const
GenericPolygon
&
other
)
{
this
->
_points
=
other
.
_points
;
return
*
this
;
}
private:
ContainerType
<
PointType
>
_points
;
};
typedef
GenericPolygon
<
geometry_msgs
::
point32
::
Point
>
Polygon
;
template
<
class
PolygonType
>
bool
toJson
(
const
PolygonType
&
poly
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
geometry_msgs
::
point32
;
rapidjson
::
Value
points
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
std
::
uint64_t
(
poly
.
points
().
size
());
++
i
)
{
rapidjson
::
Document
point
(
rapidjson
::
kObjectType
);
if
(
!
point32
::
toJson
(
poly
.
points
()[
i
],
point
,
allocator
)
){
assert
(
false
);
return
false
;
}
points
.
PushBack
(
point
,
allocator
);
}
value
.
AddMember
(
"points"
,
points
,
allocator
);
return
true
;
}
template
<
class
PolygonType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonType
&
poly
)
{
using
namespace
geometry_msgs
::
point32
;
if
(
!
value
.
HasMember
(
"points"
)
||
!
value
[
"points"
].
IsArray
()){
assert
(
false
);
return
false
;
}
const
auto
&
jsonArray
=
value
[
"points"
].
GetArray
();
poly
.
points
().
clear
();
poly
.
points
().
reserve
(
jsonArray
.
Size
());
typedef
decltype
(
poly
.
points
()[
0
])
PointTypeCVR
;
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
PointTypeCVR
>>
PointType
;
for
(
long
i
=
0
;
i
<
jsonArray
.
Size
();
++
i
){
PointType
pt
;
if
(
!
point32
::
fromJson
(
jsonArray
[
i
],
pt
)
){
assert
(
false
);
return
false
;
}
poly
.
points
().
push_back
(
std
::
move
(
pt
));
}
return
true
;
}
}
// namespace polygon
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp
0 → 100644
View file @
32d51139
#include "polygon_stamped.h"
std
::
string
ros_bridge
::
messages
::
geometry_msgs
::
polygon_stamped
::
messageType
(){
return
"geometry_msgs/PolygonStamped"
;
}
src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/polygon.h"
#include "ros_bridge/include/messages/std_msgs/header.h"
#include "ros_bridge/include/message_traits.h"
#include <type_traits>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
geometry_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation.
namespace
polygon_stamped
{
std
::
string
messageType
();
//! @brief C++ representation of geometry_msgs/PolygonStamped
template
<
class
PolygonType
=
geometry_msgs
::
polygon
::
Polygon
,
class
HeaderType
=
std_msgs
::
header
::
Header
>
class
GenericPolygonStamped
{
typedef
PolygonType
Polygon
;
public:
GenericPolygonStamped
(){}
GenericPolygonStamped
(
const
GenericPolygonStamped
&
other
)
:
_header
(
other
.
header
())
,
_polygon
(
other
.
polygon
())
{}
GenericPolygonStamped
(
const
HeaderType
&
header
,
Polygon
&
polygon
)
:
_header
(
header
)
,
_polygon
(
polygon
)
{}
const
HeaderType
&
header
()
const
{
return
_header
;}
const
Polygon
&
polygon
()
const
{
return
_polygon
;}
HeaderType
&
header
()
{
return
_header
;}
Polygon
&
polygon
()
{
return
_polygon
;}
private:
HeaderType
_header
;
Polygon
_polygon
;
};
typedef
GenericPolygonStamped
<>
PolygonStamped
;
// ===================================================================================
namespace
detail
{
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
traits
::
Int2Type
<
1
>
)
{
// polyStamped.header() exists
using
namespace
std_msgs
;
typedef
decltype
(
polyStamped
.
header
())
HeaderTypeCVR
;
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
HeaderTypeCVR
>>
HeaderType
;
HeaderType
header
;
bool
ret
=
header
::
fromJson
(
doc
,
header
);
polyStamped
.
header
()
=
header
;
return
ret
;
}
template
<
class
PolygonStampedType
>
bool
setHeader
(
const
rapidjson
::
Value
&
doc
,
PolygonStampedType
&
polyStamped
,
traits
::
Int2Type
<
0
>
)
{
// polyStamped.header() does not exists
(
void
)
doc
;
(
void
)
polyStamped
;
return
true
;
}
template
<
class
PolygonType
,
class
HeaderType
>
bool
_toJson
(
const
PolygonType
&
poly
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
std_msgs
;
using
namespace
geometry_msgs
;
rapidjson
::
Document
header
(
rapidjson
::
kObjectType
);
if
(
!
header
::
toJson
(
h
,
header
,
allocator
)){
assert
(
false
);
return
false
;
}
rapidjson
::
Document
polygon
(
rapidjson
::
kObjectType
);
if
(
!
polygon
::
toJson
(
poly
,
polygon
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"header"
,
header
,
allocator
);
value
.
AddMember
(
"polygon"
,
polygon
,
allocator
);
return
true
;
}
template
<
class
PolyStamped
,
int
k
>
bool
_toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
k
>
){
// PolyStamped has member header(), use integraded header.
return
_toJson
(
polyStamped
,
polyStamped
.
header
(),
value
,
allocator
);
}
template
<
class
PolyStamped
>
bool
_toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
0
>
){
// PolyStamped has no member header(), generate one on the fly.
using
namespace
std_msgs
::
header
;
return
_toJson
(
polyStamped
,
Header
(),
value
,
allocator
);
}
}
// namespace detail
// ===================================================================================
template
<
class
PolygonType
,
class
HeaderType
>
bool
toJson
(
const
PolygonType
&
poly
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
return
detail
::
_toJson
(
poly
,
h
,
value
,
allocator
);
}
template
<
class
PolyStamped
>
bool
toJson
(
const
PolyStamped
&
polyStamped
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
typedef
traits
::
HasMemberHeader
<
PolyStamped
>
HasHeader
;
return
detail
::
_toJson
(
polyStamped
,
value
,
allocator
,
traits
::
Int2Type
<
HasHeader
::
value
>
());
}
template
<
class
PolygonType
,
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonType
&
polyStamped
)
{
using
namespace
geometry_msgs
::
polygon
;
if
(
!
value
.
HasMember
(
"header"
)
){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"polygon"
)
){
assert
(
false
);
return
false
;
}
typedef
traits
::
HasMemberSetHeader
<
PolygonType
>
HasHeader
;
if
(
!
detail
::
setHeader
(
value
[
"header"
],
polyStamped
,
traits
::
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
if
(
!
polygon
::
fromJson
(
value
[
"polygon"
],
polyStamped
.
polygon
())
){
assert
(
false
);
return
false
;
}
return
true
;
}
}
// namespace polygon_stamped
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp
0 → 100644
View file @
32d51139
#include "polygon_array.h"
std
::
string
ros_bridge
::
messages
::
jsk_recognition_msgs
::
polygon_array
::
messageType
(){
return
"jsk_recognition_msgs/PolygonArray"
;
}
src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
#include "ros_bridge/include/messages/std_msgs/header.h"
#include <type_traits>
#include <vector>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
jsk_recognition_msgs
{
//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
namespace
polygon_array
{
std
::
string
messageType
();
//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
template
<
class
PolygonType
=
geometry_msgs
::
polygon_stamped
::
PolygonStamped
,
template
<
class
,
class
...
>
class
ContainerType
=
std
::
vector
,
class
HeaderType
=
std_msgs
::
header
::
Header
,
class
IntType
=
long
,
class
FloatType
=
double
>
class
GenericPolygonArray
{
public:
GenericPolygonArray
()
{}
GenericPolygonArray
(
const
GenericPolygonArray
&
other
)
:
_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
)
:
_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
;}
private:
HeaderType
_header
;
ContainerType
<
PolygonType
>
_polygons
;
ContainerType
<
IntType
>
_labels
;
ContainerType
<
FloatType
>
_likelihood
;
};
typedef
GenericPolygonArray
<>
PolygonArray
;
namespace
detail
{
//! Helper functions to generate Json entries for labels and likelihood.
//! \note \p p has member \fn labels().
template
<
class
PolygonArrayType
,
int
k
>
void
labelsToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
labels
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
k
>
){
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
p
.
labels
().
size
();
++
i
)
labels
.
PushBack
(
rapidjson
::
Value
().
SetUint
(
p
.
labels
()[
i
]),
allocator
);
}
//! \note \p p has no member \fn labels().
template
<
class
PolygonArrayType
>
void
labelsToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
labels
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
0
>
){
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)(
p
.
polygons
().
size
());
++
i
)
labels
.
PushBack
(
rapidjson
::
Value
().
SetUint
(
0
),
allocator
);
// use zero!
}
//! \note \p p has member \fn likelihood().
template
<
class
PolygonArrayType
,
int
k
>
void
likelihoodToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
likelyhood
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
k
>
){
p
.
likelyhood
().
clear
();
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)
p
.
likelyhood
().
size
();
++
i
)
likelyhood
.
PushBack
(
rapidjson
::
Value
().
SetFloat
(
p
.
likelyhood
()[
i
]),
allocator
);
}
//! \note \p p has no member \fn likelihood().
template
<
class
PolygonArrayType
>
void
likelihoodToJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
likelyhood
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
0
>
){
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
,
traits
::
Int2Type
<
k
>
){
p
.
labels
().
clear
();
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
,
traits
::
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
,
traits
::
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
,
traits
::
Int2Type
<
0
>
){
(
void
)
doc
;
(
void
)
p
;
}
template
<
class
PolygonArrayType
,
class
HeaderType
>
bool
_toJson
(
const
PolygonArrayType
&
p
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
std_msgs
;
using
namespace
geometry_msgs
;
rapidjson
::
Document
jHeader
(
rapidjson
::
kObjectType
);
if
(
!
header
::
toJson
(
h
,
jHeader
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"header"
,
jHeader
,
allocator
);
rapidjson
::
Value
jPolygons
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
(
unsigned
long
)(
p
.
polygons
().
size
());
++
i
){
rapidjson
::
Document
jPolygon
(
rapidjson
::
kObjectType
);
if
(
!
polygon_stamped
::
toJson
(
p
.
polygons
()[
i
].
polygon
(),
h
,
jPolygon
,
allocator
)){
assert
(
false
);
return
false
;
}
jPolygons
.
PushBack
(
jPolygon
,
allocator
);
}
value
.
AddMember
(
"polygons"
,
jPolygons
,
allocator
);
rapidjson
::
Value
jLabels
(
rapidjson
::
kArrayType
);
typedef
traits
::
HasMemberLabels
<
PolygonArrayType
>
HasLabels
;
detail
::
labelsToJson
(
p
,
jLabels
,
allocator
,
traits
::
Int2Type
<
HasLabels
::
value
>
());
value
.
AddMember
(
"labels"
,
jLabels
,
allocator
);
rapidjson
::
Value
jLikelihood
(
rapidjson
::
kArrayType
);
typedef
traits
::
HasMemberLikelihood
<
PolygonArrayType
>
HasLikelihood
;
detail
::
likelihoodToJson
(
p
,
jLikelihood
,
allocator
,
traits
::
Int2Type
<
HasLikelihood
::
value
>
());
value
.
AddMember
(
"likelihood"
,
jLikelihood
,
allocator
);
return
true
;
}
template
<
class
PolygonArrayType
,
int
k
>
bool
_toJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
k
>
)
{
// U has member header(), use integraded header.
return
_toJson
(
p
,
p
.
header
(),
value
,
allocator
);
}
template
<
class
PolygonArrayType
>
bool
_toJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
,
traits
::
Int2Type
<
0
>
)
{
// U has no member header(), generate one on the fly.
using
namespace
std_msgs
::
header
;
return
_toJson
(
p
,
Header
(),
value
,
allocator
);
}
}
//!
//! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
//! \param p Class implementing the PolygonArray interface.
//! \param h Class implementing the Header interface.
//! \param doc Rapidjson document used to store the PolygonArray message.
//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
//!
//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
//! will be replaced by arrays filled with zero and size p.polygons.size().
//!
//! \note If this function is called, the headers in p.polygons[i] (entries implement the the PolygonStamped interface)
//! are ignored.
template
<
class
PolygonArrayType
,
class
HeaderType
>
bool
toJson
(
const
PolygonArrayType
&
p
,
const
HeaderType
&
h
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
return
detail
::
_toJson
(
p
,
h
,
value
,
allocator
);
}
//!
//! Create PolygonArray message from \p p. \p p contains a header.
//! \param p Class implementing the PolygonArrayType interface.
//! \param doc Rapidjson document used to store the PolygonArray message.
//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
//!
//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
//! will be replaced by arrays filled with zero and size p.polygons.size().
//!
//! \note If the header() function is missing, a default constructed header is used.
template
<
class
PolygonArrayType
>
bool
toJson
(
const
PolygonArrayType
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
typedef
traits
::
HasMemberHeader
<
PolygonArrayType
>
HasHeader
;
return
detail
::
_toJson
(
p
,
value
,
allocator
,
traits
::
Int2Type
<
HasHeader
::
value
>
());
}
template
<
class
PolygonArrayType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
PolygonArrayType
&
p
)
{
using
namespace
geometry_msgs
;
if
(
!
value
.
HasMember
(
"header"
)){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"polygons"
)
||
!
value
[
"polygons"
].
IsArray
()
){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"labels"
)
||
!
value
[
"labels"
].
IsArray
()
){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"likelihood"
)
||
!
value
[
"likelihood"
].
IsArray
()
){
assert
(
false
);
return
false
;
}
typedef
traits
::
HasMemberHeader
<
PolygonArrayType
>
HasHeader
;
if
(
!
polygon_stamped
::
detail
::
setHeader
(
value
[
"header"
],
p
,
traits
::
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
const
auto
&
jPolygonStamped
=
value
[
"polygons"
];
p
.
polygons
().
clear
();
p
.
polygons
().
reserve
(
jPolygonStamped
.
Size
());
typedef
decltype
(
p
.
polygons
()[
0
])
PolyStampedCVR
;
typedef
typename
std
::
remove_cv_t
<
typename
std
::
remove_reference_t
<
PolyStampedCVR
>>
PolyStamped
;
for
(
unsigned
int
i
=
0
;
i
<
jPolygonStamped
.
Size
();
++
i
)
{
if
(
!
jPolygonStamped
[
i
].
HasMember
(
"header"
)
){
assert
(
false
);
return
false
;
}
PolyStamped
polygonStamped
;
if
(
!
polygon_stamped
::
detail
::
setHeader
(
jPolygonStamped
[
i
][
"header"
],
polygonStamped
,
traits
::
Int2Type
<
HasHeader
::
value
>
())){
assert
(
false
);
return
false
;
}
if
(
!
polygon
::
fromJson
(
jPolygonStamped
[
i
][
"polygon"
],
polygonStamped
.
polygon
())){
assert
(
false
);
return
false
;
}
p
.
polygons
().
push_back
(
std
::
move
(
polygonStamped
));
}
typedef
traits
::
HasMemberLabels
<
PolygonArrayType
>
HasLabels
;
detail
::
setLabels
(
value
[
"labels"
],
p
,
traits
::
Int2Type
<
HasLabels
::
value
>
());
typedef
traits
::
HasMemberLikelihood
<
PolygonArrayType
>
HasLikelihood
;
detail
::
setLikelihood
(
value
[
"likelihood"
],
p
,
traits
::
Int2Type
<
HasLikelihood
::
value
>
());
return
true
;
}
}
// namespace polygon_array
}
// namespace geometry_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp
0 → 100644
View file @
32d51139
#include "heartbeat.h"
std
::
string
ros_bridge
::
messages
::
nemo_msgs
::
heartbeat
::
messageType
(){
return
"nemo_msgs/Heartbeat"
;
}
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
nemo_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
namespace
heartbeat
{
std
::
string
messageType
();
//! @brief C++ representation of nemo_msgs/Heartbeat message
class
Heartbeat
{
public:
Heartbeat
(){}
Heartbeat
(
int
status
)
:
_status
(
status
){}
Heartbeat
(
const
Heartbeat
&
heartbeat
)
:
_status
(
heartbeat
.
status
()){}
virtual
int
status
(
void
)
const
{
return
_status
;}
virtual
void
setStatus
(
int
status
)
{
_status
=
status
;}
protected:
int
_status
;
};
template
<
class
HeartbeatType
>
bool
toJson
(
const
HeartbeatType
&
heartbeat
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"status"
,
std
::
int32_t
(
heartbeat
.
status
()),
allocator
);
return
true
;
}
template
<
class
HeartbeatType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
HeartbeatType
&
heartbeat
)
{
if
(
!
value
.
HasMember
(
"status"
)
||
!
value
[
"status"
].
IsInt
()){
assert
(
false
);
return
false
;
}
heartbeat
.
setStatus
(
value
[
"status"
].
GetInt
());
return
true
;
}
}
// namespace heartbeat
}
// namespace nemo_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp
0 → 100644
View file @
32d51139
#include "progress.h"
std
::
string
ros_bridge
::
messages
::
nemo_msgs
::
progress
::
messageType
(){
return
"nemo_msgs/Progress"
;
}
src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <vector>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace
nemo_msgs
{
//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
namespace
progress
{
std
::
string
messageType
();
//! @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
){}
GenericProgress
(
const
GenericProgress
&
p
)
:
_progress
(
p
.
progress
()){}
virtual
const
ContainterType
<
IntType
>
&
progress
(
void
)
const
{
return
_progress
;}
virtual
ContainterType
<
IntType
>
&
progress
(
void
)
{
return
_progress
;}
protected:
ContainterType
<
IntType
>
_progress
;
};
typedef
GenericProgress
<>
Progress
;
template
<
class
ProgressType
>
bool
toJson
(
const
ProgressType
&
p
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
rapidjson
::
Value
jProgress
(
rapidjson
::
kArrayType
);
for
(
unsigned
long
i
=
0
;
i
<
std
::
uint64_t
(
p
.
progress
().
size
());
++
i
){
jProgress
.
PushBack
(
rapidjson
::
Value
().
SetInt
(
std
::
int32_t
(
p
.
progress
()[
i
])),
allocator
);
}
value
.
AddMember
(
"progress"
,
jProgress
,
allocator
);
return
true
;
}
template
<
class
ProgressType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
ProgressType
&
p
)
{
if
(
!
value
.
HasMember
(
"progress"
)
||
!
value
[
"progress"
].
IsArray
()){
assert
(
false
);
return
false
;
}
const
auto
&
jsonProgress
=
value
[
"progress"
];
unsigned
long
sz
=
jsonProgress
.
Size
();
p
.
progress
().
clear
();
p
.
progress
().
reserve
(
sz
);
for
(
unsigned
long
i
=
0
;
i
<
sz
;
++
i
)
p
.
progress
().
push_back
(
std
::
int32_t
(
jsonProgress
[
i
].
GetInt
()));
return
true
;
}
}
// namespace progress
}
// namespace nemo_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/std_msgs/header.cpp
0 → 100644
View file @
32d51139
#include "header.h"
std
::
uint32_t
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
_defaultSeq
=
0
;
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Header
()
:
_seq
(
_defaultSeq
++
),
_stamp
(
Time
()),
_frameId
(
""
)
{}
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Header
(
uint32_t
seq
,
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
stamp
,
const
std
::
string
&
frame_id
)
:
_seq
(
seq
)
,
_stamp
(
stamp
)
,
_frameId
(
frame_id
)
{}
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Header
(
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
&
header
)
:
_seq
(
header
.
seq
())
,
_stamp
(
header
.
stamp
())
,
_frameId
(
header
.
frameId
())
{}
uint32_t
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
seq
()
const
{
return
_seq
;
}
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
stamp
()
const
{
return
_stamp
;
}
const
std
::
string
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
frameId
()
const
{
return
_frameId
;
}
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
stamp
()
{
return
_stamp
;
}
std
::
string
&
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
frameId
()
{
return
_frameId
;
}
void
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
setSeq
(
uint32_t
seq
)
{
_seq
=
seq
;
}
void
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
setStamp
(
const
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
Time
&
stamp
)
{
_stamp
=
stamp
;
}
void
ros_bridge
::
messages
::
std_msgs
::
header
::
Header
::
setFrameId
(
const
std
::
string
&
frameId
)
{
_frameId
=
frameId
;
}
std
::
string
ros_bridge
::
messages
::
std_msgs
::
header
::
messageType
(){
return
"std_msgs/Header"
;
}
src/comm/ros_bridge/include/messages/std_msgs/header.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/std_msgs/time.h"
#include <string>
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for std_msgs generation.
namespace
std_msgs
{
//! @brief Namespace containing classes and methodes for std_msgs/Header message generation.
namespace
header
{
std
::
string
messageType
();
//! @brief C++ representation of std_msgs/Header
class
Header
{
public:
using
Time
=
std_msgs
::
time
::
Time
;
Header
();
Header
(
uint32_t
seq
,
const
Time
&
stamp
,
const
std
::
string
&
frame_id
);
Header
(
const
Header
&
header
);
uint32_t
seq
()
const
;
const
Time
&
stamp
()
const
;
const
std
::
string
&
frameId
()
const
;
Time
&
stamp
();
std
::
string
&
frameId
();
void
setSeq
(
uint32_t
seq
);
void
setStamp
(
const
Time
&
stamp
);
void
setFrameId
(
const
std
::
string
&
frameId
);
private:
static
uint32_t
_defaultSeq
;
uint32_t
_seq
;
Time
_stamp
;
std
::
string
_frameId
;
};
template
<
class
HeaderType
>
bool
toJson
(
const
HeaderType
&
header
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
using
namespace
messages
::
std_msgs
;
value
.
AddMember
(
"seq"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
header
.
seq
())),
allocator
);
rapidjson
::
Value
stamp
(
rapidjson
::
kObjectType
);
if
(
!
time
::
toJson
(
header
.
stamp
(),
stamp
,
allocator
)){
assert
(
false
);
return
false
;
}
value
.
AddMember
(
"stamp"
,
stamp
,
allocator
);
value
.
AddMember
(
"frame_id"
,
rapidjson
::
Value
().
SetString
(
header
.
frameId
().
data
(),
header
.
frameId
().
length
(),
allocator
),
allocator
);
return
true
;
}
template
<
class
HeaderType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
HeaderType
&
header
)
{
using
namespace
messages
::
std_msgs
;
if
(
!
value
.
HasMember
(
"seq"
)
||
!
value
[
"seq"
].
IsUint
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"stamp"
)){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"frame_id"
)
||
!
value
[
"frame_id"
].
IsString
()){
assert
(
false
);
return
false
;
}
header
.
setSeq
(
value
[
"seq"
].
GetUint
());
decltype
(
header
.
stamp
())
time
;
if
(
!
time
::
fromJson
(
value
[
"stamp"
],
time
)){
assert
(
false
);
return
false
;
}
header
.
setStamp
(
time
);
header
.
setFrameId
(
value
[
"frame_id"
].
GetString
());
return
true
;
}
}
// namespace time
}
// namespace std_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/messages/std_msgs/time.cpp
0 → 100644
View file @
32d51139
#include "time.h"
std
::
string
ros_bridge
::
messages
::
std_msgs
::
time
::
messageType
(){
return
"std_msgs/Time"
;
}
src/comm/ros_bridge/include/messages/std_msgs/time.h
0 → 100644
View file @
32d51139
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace
ros_bridge
{
//! @brief Namespace containing classes and methodes ros message generation.
namespace
messages
{
//! @brief Namespace containing classes and methodes for std_msgs generation.
namespace
std_msgs
{
//! @brief Namespace containing classes and methodes for std_msgs/Time message generation.
namespace
time
{
std
::
string
messageType
();
//! @brief C++ representation of std_msgs/Time
class
Time
{
public:
Time
()
:
_secs
(
0
),
_nsecs
(
0
)
{}
Time
(
uint32_t
secs
,
uint32_t
nsecs
)
:
_secs
(
secs
),
_nsecs
(
nsecs
)
{}
Time
(
const
Time
&
time
)
:
_secs
(
time
.
secs
()),
_nsecs
(
time
.
nSecs
())
{}
uint32_t
secs
()
const
{
return
_secs
;}
uint32_t
nSecs
()
const
{
return
_nsecs
;}
void
setSecs
(
uint32_t
secs
)
{
_secs
=
secs
;}
void
setNSecs
(
uint32_t
nsecs
)
{
_nsecs
=
nsecs
;}
private:
uint32_t
_secs
;
uint32_t
_nsecs
;
};
template
<
class
TimeType
>
bool
toJson
(
const
TimeType
&
time
,
rapidjson
::
Value
&
value
,
rapidjson
::
Document
::
AllocatorType
&
allocator
)
{
value
.
AddMember
(
"secs"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
time
.
secs
())),
allocator
);
value
.
AddMember
(
"nsecs"
,
rapidjson
::
Value
().
SetUint
(
uint32_t
(
time
.
nSecs
())),
allocator
);
return
true
;
}
template
<
class
TimeType
>
bool
fromJson
(
const
rapidjson
::
Value
&
value
,
TimeType
&
time
)
{
if
(
!
value
.
HasMember
(
"secs"
)
||
!
value
[
"secs"
].
IsUint
()){
assert
(
false
);
return
false
;
}
if
(
!
value
.
HasMember
(
"nsecs"
)
||
!
value
[
"nsecs"
].
IsUint
()){
assert
(
false
);
return
false
;
}
time
.
setSecs
(
value
[
"secs"
].
GetUint
());
time
.
setNSecs
(
value
[
"nsecs"
].
GetUint
());
return
true
;
}
}
// namespace time
}
// namespace std_msgs
}
// namespace messages
}
// namespace ros_bridge
src/comm/ros_bridge/include/
ROSB
ridge.h
→
src/comm/ros_bridge/include/
ros_b
ridge.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/CasePacker.h"
#include "ros_bridge/include/topic_publisher.h"
#include "ros_bridge/include/TypeFactory.h"
#include "ros_bridge/include/topic_subscriber.h"
#include "ros_bridge/include/JsonFactory.h"
#include "ros_bridge/include/server.h"
#include "ros_bridge/include/TopicPublisher.h"
#include "ros_bridge/include/TopicSubscriber.h"
#include "ros_bridge/include/Server.h"
#include <memory>
#include <memory>
#include <functional>
#include <functional>
namespace
ROSB
ridge
{
namespace
ros_b
ridge
{
class
ROSBridge
class
ROSBridge
{
{
public:
public:
typedef
MessageTag
Tag
;
typedef
rapidjson
::
Document
JsonDoc
;
typedef
rapidjson
::
Document
JsonDoc
;
typedef
std
::
unique_ptr
<
JsonDoc
>
JsonDocUPtr
;
typedef
std
::
unique_ptr
<
JsonDoc
>
JsonDocUPtr
;
explicit
ROSBridge
();
explicit
ROSBridge
();
explicit
ROSBridge
(
const
char
*
connectionString
);
explicit
ROSBridge
(
const
char
*
connectionString
);
void
publish
(
JsonDocUPtr
doc
,
const
char
*
topic
);
template
<
class
T
>
void
publish
(
T
&
msg
,
const
std
::
string
&
topic
){
_topicPublisher
.
publish
(
msg
,
topic
);
}
void
publish
(
JsonDocUPtr
doc
);
void
subscribe
(
const
char
*
topic
,
void
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
);
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
);
void
advertiseService
(
const
std
::
string
&
service
,
void
advertiseService
(
const
char
*
service
,
const
std
::
string
&
type
,
const
char
*
type
,
const
std
::
function
<
JsonDocUPtr
(
JsonDocUPtr
)
>
&
callback
);
const
std
::
function
<
JsonDocUPtr
(
JsonDocUPtr
)
>
&
callback
);
void
advertiseTopic
(
const
char
*
topic
,
const
CasePacker
*
casePacker
()
const
;
const
char
*
type
)
;
//! @brief Start ROS bridge.
//! @brief Start ROS bridge.
void
start
();
void
start
();
//! @brief Stops ROS bridge.
//! @brief Stops ROS bridge.
void
reset
();
void
reset
();
//! @return Returns true if connected to the rosbridge_server, false either.
//! @return Returns true if connected to the rosbridge_server, false either.
//! @note
This function can block up to 100ms. However normal execution time is smaller
.
//! @note
\fn calls start()
.
bool
connected
();
bool
connected
();
bool
isRunning
();
bool
isRunning
();
private:
private:
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
TypeFactory
_typeFactory
;
RosbridgeWsClient
_rbc
;
JsonFactory
_jsonFactory
;
com_private
::
TopicPublisher
_topicPublisher
;
CasePacker
_casePacker
;
com_private
::
TopicSubscriber
_topicSubscriber
;
RosbridgeWsClient
_rbc
;
com_private
::
Server
_server
;
ComPrivate
::
TopicPublisher
_topicPublisher
;
ComPrivate
::
TopicSubscriber
_topicSubscriber
;
ComPrivate
::
Server
_server
;
};
};
...
...
src/comm/ros_bridge/include/
S
erver.cpp
→
src/comm/ros_bridge/include/
s
erver.cpp
View file @
32d51139
#include "
S
erver.h"
#include "
s
erver.h"
#include "rapidjson/include/rapidjson/ostreamwrapper.h"
#include "rapidjson/include/rapidjson/ostreamwrapper.h"
ROSBridge
::
ComPrivate
::
Server
::
Server
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
)
:
ros_bridge
::
com_private
::
Server
::
Server
(
RosbridgeWsClient
&
rbc
)
:
_rbc
(
rbc
)
_rbc
(
rbc
)
,
_casePacker
(
casePacker
)
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
{
}
}
void
ROSBridge
::
ComP
rivate
::
Server
::
advertiseService
(
const
std
::
string
&
service
,
void
ros_bridge
::
com_p
rivate
::
Server
::
advertiseService
(
const
std
::
string
&
service
,
const
std
::
string
&
type
,
const
std
::
string
&
type
,
const
Server
::
CallbackType
&
userCallback
)
const
Server
::
CallbackType
&
userCallback
)
{
{
...
@@ -22,9 +21,8 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
...
@@ -22,9 +21,8 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
_rbc
.
addClient
(
clientName
);
_rbc
.
addClient
(
clientName
);
auto
rbc
=
&
_rbc
;
auto
rbc
=
&
_rbc
;
auto
casePacker
=
&
_casePacker
;
_rbc
.
advertiseService
(
clientName
,
service
,
type
,
_rbc
.
advertiseService
(
clientName
,
service
,
type
,
[
userCallback
,
rbc
,
casePacker
](
[
userCallback
,
rbc
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
// message->string() is destructive, so we have to buffer it first
// message->string() is destructive, so we have to buffer it first
...
@@ -74,17 +72,17 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
...
@@ -74,17 +72,17 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
});
});
}
}
ROSBridge
::
ComP
rivate
::
Server
::~
Server
()
ros_bridge
::
com_p
rivate
::
Server
::~
Server
()
{
{
this
->
reset
();
this
->
reset
();
}
}
void
ROSBridge
::
ComP
rivate
::
Server
::
start
()
void
ros_bridge
::
com_p
rivate
::
Server
::
start
()
{
{
_stopped
->
store
(
false
);
_stopped
->
store
(
false
);
}
}
void
ROSBridge
::
ComP
rivate
::
Server
::
reset
()
void
ros_bridge
::
com_p
rivate
::
Server
::
reset
()
{
{
if
(
_stopped
->
load
()
)
if
(
_stopped
->
load
()
)
return
;
return
;
...
...
src/comm/ros_bridge/include/
S
erver.h
→
src/comm/ros_bridge/include/
s
erver.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/
ComPrivateInclud
e.h"
#include "ros_bridge/include/
com_privat
e.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/TypeFactory.h"
#include "ros_bridge/include/CasePacker.h"
#include <unordered_map>
#include <unordered_map>
namespace
ROSB
ridge
{
namespace
ros_b
ridge
{
namespace
ComP
rivate
{
namespace
com_p
rivate
{
class
Server
class
Server
{
{
...
@@ -18,7 +16,7 @@ public:
...
@@ -18,7 +16,7 @@ public:
Server
()
=
delete
;
Server
()
=
delete
;
Server
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
);
Server
(
RosbridgeWsClient
&
rbc
);
~
Server
();
~
Server
();
//! @brief Starts the subscriber.
//! @brief Starts the subscriber.
...
@@ -34,7 +32,6 @@ public:
...
@@ -34,7 +32,6 @@ public:
private:
private:
RosbridgeWsClient
&
_rbc
;
RosbridgeWsClient
&
_rbc
;
CasePacker
&
_casePacker
;
ServiceMap
_serviceMap
;
ServiceMap
_serviceMap
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
};
};
...
...
src/comm/ros_bridge/include/topic_publisher.cpp
0 → 100644
View file @
32d51139
#include "topic_publisher.h"
#include <unordered_map>
ros_bridge
::
com_private
::
TopicPublisher
::
TopicPublisher
(
RosbridgeWsClient
&
rbc
)
:
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_rbc
(
rbc
)
{
}
ros_bridge
::
com_private
::
TopicPublisher
::~
TopicPublisher
()
{
this
->
reset
();
}
void
ros_bridge
::
com_private
::
TopicPublisher
::
start
()
{
if
(
!
_stopped
->
load
()
)
// start called while thread running.
return
;
_stopped
->
store
(
false
);
_pThread
=
std
::
make_unique
<
std
::
thread
>
([
this
]{
// Main Loop.
while
(
!
this
->
_stopped
->
load
()
){
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
// Check if new data available, wait if not.
if
(
this
->
_queue
.
empty
()){
if
(
_stopped
->
load
()
)
// Check condition again while holding the lock.
break
;
this
->
_cv
.
wait
(
lk
);
// Wait for condition, spurious wakeups don't matter in this case.
continue
;
}
// Pop message from queue.
JsonDocUPtr
pJsonDoc
=
std
::
move
(
this
->
_queue
.
front
());
this
->
_queue
.
pop_front
();
lk
.
unlock
();
// Get topic and type from Json message and remove it.
std
::
string
topic
;
bool
ret
=
com_private
::
getTopic
(
*
pJsonDoc
,
topic
);
assert
(
ret
);
(
void
)
ret
;
// Debug
rapidjson
::
StringBuffer
sb
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
sb
);
pJsonDoc
->
Accept
(
writer
);
std
::
cout
<<
"message: "
<<
sb
.
GetString
()
<<
std
::
endl
;
std
::
cout
<<
"topic: "
<<
topic
<<
std
::
endl
;
ret
=
com_private
::
removeTopic
(
*
pJsonDoc
);
assert
(
ret
);
(
void
)
ret
;
// Wait for topic (Rosbridge needs some time to process a advertise() request).
this
->
_rbc
.
waitForTopic
(
topic
,
[
this
]{
return
this
->
_stopped
->
load
();
});
// Publish Json message.
if
(
!
this
->
_stopped
->
load
()
)
this
->
_rbc
.
publish
(
topic
,
*
pJsonDoc
);
}
// while loop
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
"TopicPublisher: publisher thread terminated."
<<
std
::
endl
;
#endif
});
}
void
ros_bridge
::
com_private
::
TopicPublisher
::
reset
()
{
if
(
_stopped
->
load
()
)
// stop called while thread not running.
return
;
std
::
unique_lock
<
std
::
mutex
>
lk
(
_mutex
);
_stopped
->
store
(
true
);
_cv
.
notify_one
();
// Wake publisher thread.
lk
.
unlock
();
if
(
!
_pThread
)
return
;
_pThread
->
join
();
lk
.
lock
();
// Tidy up.
for
(
auto
&
it
:
this
->
_topicMap
){
this
->
_rbc
.
unadvertise
(
it
.
first
);
this
->
_rbc
.
removeClient
(
it
.
second
);
}
this
->
_topicMap
.
clear
();
_queue
.
clear
();
}
void
ros_bridge
::
com_private
::
TopicPublisher
::
publish
(
ros_bridge
::
com_private
::
JsonDocUPtr
docPtr
,
const
char
*
topic
)
{
// Check if topic was advertised.
std
::
string
t
(
topic
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
auto
it
=
this
->
_topicMap
.
find
(
t
);
if
(
it
==
this
->
_topicMap
.
end
())
{
// Not yet advertised?
lk
.
unlock
();
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"TopicPublisher: topic "
<<
t
<<
" not yet advertised"
<<
std
::
endl
;
#endif
return
;
}
lk
.
unlock
();
// Add topic information to json doc.
auto
&
allocator
=
docPtr
->
GetAllocator
();
rapidjson
::
Value
key
(
"topic"
,
allocator
);
rapidjson
::
Value
value
(
topic
,
allocator
);
docPtr
->
AddMember
(
key
,
value
,
allocator
);
lk
.
lock
();
_queue
.
push_back
(
std
::
move
(
docPtr
));
lk
.
unlock
();
_cv
.
notify_one
();
// Wake publisher thread.
}
bool
ros_bridge
::
com_private
::
TopicPublisher
::
advertise
(
const
char
*
topic
,
const
char
*
type
)
{
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
std
::
string
t
(
topic
);
auto
it
=
this
->
_topicMap
.
find
(
t
);
if
(
it
==
this
->
_topicMap
.
end
())
{
// Need to advertise topic?
std
::
string
clientName
=
std
::
string
(
ros_bridge
::
com_private
::
_topicAdvertiserKey
)
+
t
;
this
->
_topicMap
.
insert
(
std
::
make_pair
(
t
,
clientName
));
lk
.
unlock
();
this
->
_rbc
.
addClient
(
clientName
);
this
->
_rbc
.
advertise
(
clientName
,
t
,
type
);
return
true
;
}
else
{
lk
.
unlock
();
#if ROS_BRIDGE_DEBUG
std
::
cerr
<<
"TopicPublisher: topic "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
#endif
return
false
;
}
}
src/comm/ros_bridge/include/
TopicP
ublisher.h
→
src/comm/ros_bridge/include/
topic_p
ublisher.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/TypeFactory.h"
#include "ros_bridge/include/com_private.h"
#include "ros_bridge/include/CasePacker.h"
#include "ros_bridge/include/ComPrivateInclude.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include <thread>
#include <thread>
#include <deque>
#include <atomic>
#include <atomic>
#include <mutex>
#include <mutex>
#include <set>
#include <condition_variable>
#include <condition_variable>
namespace
ROSB
ridge
{
namespace
ros_b
ridge
{
namespace
ComP
rivate
{
namespace
com_p
rivate
{
struct
ThreadData
;
struct
ThreadData
;
...
@@ -24,8 +20,7 @@ class TopicPublisher
...
@@ -24,8 +20,7 @@ class TopicPublisher
public:
public:
TopicPublisher
()
=
delete
;
TopicPublisher
()
=
delete
;
TopicPublisher
(
CasePacker
&
casePacker
,
TopicPublisher
(
RosbridgeWsClient
&
rbc
);
RosbridgeWsClient
&
rbc
);
~
TopicPublisher
();
~
TopicPublisher
();
...
@@ -35,25 +30,15 @@ public:
...
@@ -35,25 +30,15 @@ public:
//! @brief Resets the publisher.
//! @brief Resets the publisher.
void
reset
();
void
reset
();
void
publish
(
JsonDocUPtr
docPtr
){
void
publish
(
JsonDocUPtr
docPtr
,
const
char
*
topic
);
{
bool
advertise
(
const
char
*
topic
,
const
char
*
type
);
std
::
lock_guard
<
std
::
mutex
>
lock
(
_mutex
);
_queue
.
push_back
(
std
::
move
(
docPtr
));
}
_cv
.
notify_one
();
// Wake publisher thread.
}
template
<
class
T
>
void
publish
(
const
T
&
msg
,
const
std
::
string
&
topic
){
JsonDocUPtr
docPtr
(
_casePacker
.
pack
(
msg
,
topic
));
publish
(
std
::
move
(
docPtr
));
}
private:
private:
using
TopicMap
=
std
::
unordered_map
<
std
::
string
,
std
::
string
>
;
JsonQueue
_queue
;
JsonQueue
_queue
;
TopicMap
_topicMap
;
std
::
mutex
_mutex
;
std
::
mutex
_mutex
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
CasePacker
&
_casePacker
;
RosbridgeWsClient
&
_rbc
;
RosbridgeWsClient
&
_rbc
;
CondVar
_cv
;
CondVar
_cv
;
ThreadPtr
_pThread
;
ThreadPtr
_pThread
;
...
...
src/comm/ros_bridge/include/
TopicS
ubscriber.cpp
→
src/comm/ros_bridge/include/
topic_s
ubscriber.cpp
View file @
32d51139
#include "
TopicS
ubscriber.h"
#include "
topic_s
ubscriber.h"
#include <thread>
#include <thread>
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
TopicSubscriber
(
CasePacker
&
casePacker
,
ros_bridge
::
com_private
::
TopicSubscriber
::
TopicSubscriber
(
RosbridgeWsClient
&
rbc
)
:
RosbridgeWsClient
&
rbc
)
:
_rbc
(
rbc
)
_casePacker
(
casePacker
)
,
_rbc
(
rbc
)
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
{
}
}
ROSBridge
::
ComP
rivate
::
TopicSubscriber
::~
TopicSubscriber
()
ros_bridge
::
com_p
rivate
::
TopicSubscriber
::~
TopicSubscriber
()
{
{
this
->
reset
();
this
->
reset
();
}
}
void
ROSBridge
::
ComP
rivate
::
TopicSubscriber
::
start
()
void
ros_bridge
::
com_p
rivate
::
TopicSubscriber
::
start
()
{
{
_stopped
->
store
(
false
);
_stopped
->
store
(
false
);
}
}
void
ROSBridge
::
ComP
rivate
::
TopicSubscriber
::
reset
()
void
ros_bridge
::
com_p
rivate
::
TopicSubscriber
::
reset
()
{
{
if
(
!
_stopped
->
load
()
){
if
(
!
_stopped
->
load
()
){
std
::
cout
<<
"TopicSubscriber: _stopped->store(true) "
<<
std
::
endl
;
_stopped
->
store
(
true
);
_stopped
->
store
(
true
);
{
{
for
(
auto
&
item
:
_topicMap
){
for
(
auto
&
item
:
_topicMap
){
std
::
cout
<<
"TopicSubscriber: unsubscribe "
<<
item
.
second
<<
std
::
endl
;
_rbc
.
unsubscribe
(
item
.
second
);
_rbc
.
unsubscribe
(
item
.
second
);
std
::
cout
<<
"TopicSubscriber: removeClient "
<<
item
.
first
<<
std
::
endl
;
_rbc
.
removeClient
(
item
.
first
);
_rbc
.
removeClient
(
item
.
first
);
}
}
}
}
_topicMap
.
clear
();
_topicMap
.
clear
();
std
::
cout
<<
"TopicSubscriber: _topicMap cleared "
<<
std
::
endl
;
}
}
}
}
void
ROSBridge
::
ComP
rivate
::
TopicSubscriber
::
subscribe
(
void
ros_bridge
::
com_p
rivate
::
TopicSubscriber
::
subscribe
(
const
char
*
topic
,
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
{
{
if
(
_stopped
->
load
()
)
if
(
_stopped
->
load
()
)
return
;
return
;
std
::
string
clientName
=
ROSBridge
::
ComP
rivate
::
_topicSubscriberKey
std
::
string
clientName
=
ros_bridge
::
com_p
rivate
::
_topicSubscriberKey
+
std
::
string
(
topic
);
+
std
::
string
(
topic
);
auto
it
=
_topicMap
.
find
(
clientName
);
auto
it
=
_topicMap
.
find
(
clientName
);
if
(
it
!=
_topicMap
.
end
()
){
// Topic already subscribed?
if
(
it
!=
_topicMap
.
end
()
){
// Topic already subscribed?
...
@@ -55,7 +49,6 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
...
@@ -55,7 +49,6 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
_topicMap
.
insert
(
std
::
make_pair
(
clientName
,
std
::
string
(
topic
)));
_topicMap
.
insert
(
std
::
make_pair
(
clientName
,
std
::
string
(
topic
)));
// Wrap callback.
// Wrap callback.
using
namespace
std
::
placeholders
;
auto
callbackWrapper
=
[
callback
](
auto
callbackWrapper
=
[
callback
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
...
...
src/comm/ros_bridge/include/
TopicS
ubscriber.h
→
src/comm/ros_bridge/include/
topic_s
ubscriber.h
View file @
32d51139
#pragma once
#pragma once
#include "ros_bridge/include/
ComPrivateInclud
e.h"
#include "ros_bridge/include/
com_privat
e.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/TypeFactory.h"
#include "ros_bridge/include/CasePacker.h"
#include <unordered_map>
#include <unordered_map>
namespace
ROSB
ridge
{
namespace
ros_b
ridge
{
namespace
ComP
rivate
{
namespace
com_p
rivate
{
typedef
std
::
function
<
void
(
JsonDocUPtr
)
>
CallbackType
;
typedef
std
::
function
<
void
(
JsonDocUPtr
)
>
CallbackType
;
...
@@ -19,7 +17,7 @@ class TopicSubscriber
...
@@ -19,7 +17,7 @@ class TopicSubscriber
public:
public:
TopicSubscriber
()
=
delete
;
TopicSubscriber
()
=
delete
;
TopicSubscriber
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
);
TopicSubscriber
(
RosbridgeWsClient
&
rbc
);
~
TopicSubscriber
();
~
TopicSubscriber
();
//! @brief Starts the subscriber.
//! @brief Starts the subscriber.
...
@@ -34,11 +32,7 @@ public:
...
@@ -34,11 +32,7 @@ public:
void
subscribe
(
const
char
*
topic
,
const
CallbackType
&
callback
);
void
subscribe
(
const
char
*
topic
,
const
CallbackType
&
callback
);
private:
private:
RosbridgeWsClient
&
_rbc
;
CasePacker
&
_casePacker
;
RosbridgeWsClient
&
_rbc
;
TopicMap
_topicMap
;
TopicMap
_topicMap
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
};
};
...
...
src/comm/ros_bridge/src/CasePacker.cpp
deleted
100644 → 0
View file @
9a7d98f8
#include "ros_bridge/include/CasePacker.h"
const
char
*
ROSBridge
::
CasePacker
::
topicKey
=
"topic"
;
const
char
*
ROSBridge
::
CasePacker
::
messageTypeKey
=
"messageType"
;
ROSBridge
::
CasePacker
::
CasePacker
(
TypeFactory
*
typeFactory
,
JsonFactory
*
jsonFactory
)
:
_typeFactory
(
typeFactory
)
,
_jsonFactory
(
jsonFactory
)
{
}
bool
ROSBridge
::
CasePacker
::
getTag
(
const
JsonDocUPtr
&
pDoc
,
Tag
&
tag
)
const
{
if
(
!
getTopic
(
pDoc
,
tag
.
topic
())
)
return
false
;
if
(
!
getMessageType
(
pDoc
,
tag
.
messageType
())
)
return
false
;
return
true
;
}
void
ROSBridge
::
CasePacker
::
addTag
(
JsonDocUPtr
&
pDoc
,
const
std
::
string
&
topic
,
const
std
::
string
&
messageType
)
const
{
using
namespace
ROSBridge
;
using
namespace
rapidjson
;
{
// add topic
rapidjson
::
Value
key
(
CasePacker
::
topicKey
,
pDoc
->
GetAllocator
());
rapidjson
::
Value
value
(
topic
.
c_str
(),
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
key
,
value
,
pDoc
->
GetAllocator
());
}
// add messageType
rapidjson
::
Value
key
(
CasePacker
::
messageTypeKey
,
pDoc
->
GetAllocator
());
rapidjson
::
Value
value
(
messageType
.
c_str
(),
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
key
,
value
,
pDoc
->
GetAllocator
());
}
void
ROSBridge
::
CasePacker
::
addTag
(
JsonDocUPtr
&
doc
,
const
ROSBridge
::
CasePacker
::
Tag
&
tag
)
const
{
addTag
(
doc
,
tag
.
topic
(),
tag
.
messageType
());
}
void
ROSBridge
::
CasePacker
::
removeTag
(
JsonDocUPtr
&
pDoc
)
const
{
using
namespace
ROSBridge
;
using
namespace
rapidjson
;
if
(
pDoc
->
HasMember
(
CasePacker
::
topicKey
)
)
pDoc
->
RemoveMember
(
CasePacker
::
topicKey
);
if
(
pDoc
->
HasMember
(
CasePacker
::
messageTypeKey
)
)
pDoc
->
RemoveMember
(
CasePacker
::
messageTypeKey
);
}
bool
ROSBridge
::
CasePacker
::
getTopic
(
const
JsonDocUPtr
&
pDoc
,
std
::
string
&
topic
)
const
{
if
(
!
pDoc
->
HasMember
(
CasePacker
::
topicKey
)
||
!
(
*
pDoc
)[
CasePacker
::
topicKey
].
IsString
())
return
false
;
topic
=
(
*
pDoc
)[
CasePacker
::
topicKey
].
GetString
();
return
true
;
}
bool
ROSBridge
::
CasePacker
::
getMessageType
(
const
JsonDocUPtr
&
pDoc
,
std
::
string
&
messageType
)
const
{
if
(
!
pDoc
->
HasMember
(
CasePacker
::
messageTypeKey
)
||
!
(
*
pDoc
)[
CasePacker
::
messageTypeKey
].
IsString
())
return
false
;
messageType
=
(
*
pDoc
)[
CasePacker
::
messageTypeKey
].
GetString
();
return
true
;
}
src/comm/ros_bridge/src/
ROSB
ridge.cpp
→
src/comm/ros_bridge/src/
ros_b
ridge.cpp
View file @
32d51139
#include "ros_bridge/include/
ROSB
ridge.h"
#include "ros_bridge/include/
ros_b
ridge.h"
ROSBridge
::
ROSBridge
::
ROSBridge
(
const
char
*
connectionString
)
:
ros_bridge
::
ROSBridge
::
ROSBridge
(
const
char
*
connectionString
)
:
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_casePacker
(
&
_typeFactory
,
&
_jsonFactory
)
,
_rbc
(
connectionString
,
false
/*run*/
)
,
_rbc
(
connectionString
,
false
/*run*/
)
,
_topicPublisher
(
_
casePacker
,
_
rbc
)
,
_topicPublisher
(
_rbc
)
,
_topicSubscriber
(
_
casePacker
,
_
rbc
)
,
_topicSubscriber
(
_rbc
)
,
_server
(
_
casePacker
,
_
rbc
)
,
_server
(
_rbc
)
{
{
}
}
ROSB
ridge
::
ROSBridge
::
ROSBridge
()
:
ros_b
ridge
::
ROSBridge
::
ROSBridge
()
:
ROSB
ridge
::
ROSBridge
::
ROSBridge
(
"localhost:9090"
)
ros_b
ridge
::
ROSBridge
::
ROSBridge
(
"localhost:9090"
)
{
{
}
}
void
ROSBridge
::
ROSBridge
::
publish
(
ROSBridge
::
ROSBridge
::
JsonDocUPtr
do
c
)
void
ros_bridge
::
ROSBridge
::
publish
(
ros_bridge
::
ROSBridge
::
JsonDocUPtr
doc
,
const
char
*
topi
c
)
{
{
_topicPublisher
.
publish
(
std
::
move
(
doc
));
_topicPublisher
.
publish
(
std
::
move
(
doc
)
,
topic
);
}
}
void
ROSB
ridge
::
ROSBridge
::
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
)
void
ros_b
ridge
::
ROSBridge
::
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
)
{
{
_topicSubscriber
.
subscribe
(
topic
,
callBack
);
_topicSubscriber
.
subscribe
(
topic
,
callBack
);
}
}
void
ROSBridge
::
ROSBridge
::
advertiseService
(
const
std
::
string
&
service
,
void
ros_bridge
::
ROSBridge
::
advertiseService
(
const
char
*
service
,
const
std
::
string
&
type
,
const
char
*
type
,
const
std
::
function
<
JsonDocUPtr
(
JsonDocUPtr
)
>
&
callback
)
const
std
::
function
<
JsonDocUPtr
(
JsonDocUPtr
)
>
&
callback
)
{
{
_server
.
advertiseService
(
service
,
type
,
callback
);
_server
.
advertiseService
(
service
,
type
,
callback
);
}
}
const
ROSBridge
::
CasePacker
*
ROSBridge
::
ROSBridge
::
casePacker
()
const
void
ros_bridge
::
ROSBridge
::
advertiseTopic
(
const
char
*
topic
,
const
char
*
type
)
{
{
return
&
_casePacker
;
_topicPublisher
.
advertise
(
topic
,
type
)
;
}
}
void
ROSB
ridge
::
ROSBridge
::
start
()
void
ros_b
ridge
::
ROSBridge
::
start
()
{
{
if
(
!
_stopped
->
load
()
)
return
;
_stopped
->
store
(
false
);
_stopped
->
store
(
false
);
_rbc
.
run
();
_rbc
.
run
();
_topicPublisher
.
start
();
_topicPublisher
.
start
();
...
@@ -46,9 +48,11 @@ void ROSBridge::ROSBridge::start()
...
@@ -46,9 +48,11 @@ void ROSBridge::ROSBridge::start()
_server
.
start
();
_server
.
start
();
}
}
void
ROSB
ridge
::
ROSBridge
::
reset
()
void
ros_b
ridge
::
ROSBridge
::
reset
()
{
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
if
(
_stopped
->
load
()
)
return
;
_stopped
->
store
(
true
);
_stopped
->
store
(
true
);
_topicPublisher
.
reset
();
_topicPublisher
.
reset
();
_topicSubscriber
.
reset
();
_topicSubscriber
.
reset
();
...
@@ -56,21 +60,22 @@ void ROSBridge::ROSBridge::reset()
...
@@ -56,21 +60,22 @@ void ROSBridge::ROSBridge::reset()
_rbc
.
reset
();
_rbc
.
reset
();
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
std
::
cout
<<
"
ROSB
ridge reset duration: "
<<
delta
<<
" ms"
<<
std
::
endl
;
std
::
cout
<<
"
ros_b
ridge reset duration: "
<<
delta
<<
" ms"
<<
std
::
endl
;
}
}
bool
ROSB
ridge
::
ROSBridge
::
connected
()
bool
ros_b
ridge
::
ROSBridge
::
connected
()
{
{
start
();
return
_rbc
.
connected
();
return
_rbc
.
connected
();
}
}
bool
ROSB
ridge
::
ROSBridge
::
isRunning
()
bool
ros_b
ridge
::
ROSBridge
::
isRunning
()
{
{
return
!
_stopped
->
load
();
return
!
_stopped
->
load
();
}
}
bool
ROSB
ridge
::
isValidConnectionString
(
const
char
*
connectionString
)
bool
ros_b
ridge
::
isValidConnectionString
(
const
char
*
connectionString
)
{
{
return
is_valid_port_path
(
connectionString
);
return
is_valid_port_path
(
connectionString
);
}
}
src/comm/utilities.h
View file @
32d51139
...
@@ -73,15 +73,4 @@ private:
...
@@ -73,15 +73,4 @@ private:
bool
_isInitialized
;
bool
_isInitialized
;
};
};
template
<
typename
T
>
struct
Type2Type
{
typedef
T
OriginalType
;
};
template
<
int
k
>
struct
Int2Type
{
enum
{
value
=
k
};
};
#endif // UTILITIES_H
#endif // UTILITIES_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