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
2430ec45
Commit
2430ec45
authored
Aug 26, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Plain Diff
merged with phil
parents
0e34b591
1081ec45
Changes
69
Show whitespace changes
Inline
Side-by-side
Showing
69 changed files
with
3488 additions
and
2989 deletions
+3488
-2989
AppImageCommitHash
deploy/AppImageCommitHash
+0
-1
QGroundControl.AppImage
deploy/QGroundControl.AppImage
+0
-0
appImageCommand.txt
deploy/appImageCommand.txt
+1
-1
create_linux_appimage.sh
deploy/create_linux_appimage.sh
+6
-6
qgroundcontrol.pro
qgroundcontrol.pro
+37
-21
Wima.SettingsGroup.json
src/Settings/Wima.SettingsGroup.json
+6
-0
WimaSettings.cc
src/Settings/WimaSettings.cc
+1
-0
WimaSettings.h
src/Settings/WimaSettings.h
+1
-0
GeoPoint3D.cpp
src/Wima/Geometry/GeoPoint3D.cpp
+7
-12
GeoPoint3D.h
src/Wima/Geometry/GeoPoint3D.h
+11
-17
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
PolygonArray.h
src/Wima/Snake/PolygonArray.h
+1
-3
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
WimaController.cc
src/Wima/WimaController.cc
+283
-126
WimaController.h
src/Wima/WimaController.h
+11
-13
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
+970
-0
RosBridgeClient.h
src/comm/ros_bridge/include/RosBridgeClient.h
+150
-467
ThreadSafeQueue.h
src/comm/ros_bridge/include/ThreadSafeQueue.h
+0
-79
TopicPublisher.cpp
src/comm/ros_bridge/include/TopicPublisher.cpp
+0
-109
TopicSubscriber.cpp
src/comm/ros_bridge/include/TopicSubscriber.cpp
+0
-136
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
+9
-5
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
+99
-0
server.h
src/comm/ros_bridge/include/server.h
+39
-0
topic_publisher.cpp
src/comm/ros_bridge/include/topic_publisher.cpp
+147
-0
topic_publisher.h
src/comm/ros_bridge/include/topic_publisher.h
+49
-0
topic_subscriber.cpp
src/comm/ros_bridge/include/topic_subscriber.cpp
+99
-0
topic_subscriber.h
src/comm/ros_bridge/include/topic_subscriber.h
+41
-0
CasePacker.cpp
src/comm/ros_bridge/src/CasePacker.cpp
+0
-69
PackageBuffer.h
src/comm/ros_bridge/src/PackageBuffer.h
+0
-45
ROSBridge.cpp
src/comm/ros_bridge/src/ROSBridge.cpp
+0
-43
ros_bridge.cpp
src/comm/ros_bridge/src/ros_bridge.cpp
+81
-0
utilities.h
src/comm/utilities.h
+0
-11
GeneralSettings.qml
src/ui/preferences/GeneralSettings.qml
+17
-3
No files found.
deploy/AppImageCommitHash
deleted
100644 → 0
View file @
0e34b591
bd79747292138b2ea0
deploy/QGroundControl.AppImage
0 → 100755
View file @
2430ec45
File added
deploy/appImageCommand.txt
View file @
2430ec45
./create_linux_appimage.sh
/home/valentin/Desktop/drones/qgroundcontrol/ /home/valentin/Desktop/drones/build-qgroundcontrol-Desktop_Qt_5_11_3_GCC_64bit-Release/release/
./create_linux_appimage.sh
~/Desktop/drones/qgroundcontrol ~/Desktop/drones/build-qgroundcontrol-Desktop_Qt_5_11_3_GCC_64bit-Release/release
deploy/create_linux_appimage.sh
View file @
2430ec45
...
...
@@ -66,19 +66,19 @@ dpkg -x libts-0.0-0_1.0-11_amd64.deb libts
cp
-L
libts/usr/lib/x86_64-linux-gnu/libts-0.0.so.0
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
# copy libortools.so, etc...
cp
-L
${
QGC_SRC
}
/libs/or-tools-src-ubuntu/lib/
*
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
cp
-L
${
QGC_SRC
}
/libs/or-tools-src-ubuntu/lib/
*
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
||
{
echo
"libortools.so not found"
;
exit
1
;
}
# copy libGeographic.so
cp
-L
/usr/lib/x86_64-linux-gnu/libGeographic.so
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
# copy libGeographic.so
.17
cp
-L
/usr/lib/x86_64-linux-gnu/libGeographic.so
.17
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
||
{
echo
"libGeographic.so.17 not found"
;
exit
1
;
}
# copy boost
cp
-L
/usr/lib/x86_64-linux-gnu/libboost_system.so.1.65.1
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
cp
-L
/usr/lib/x86_64-linux-gnu/libboost_system.so.1.65.1
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
||
{
echo
"libboost_system.so.1.65.1 not found"
;
exit
1
;
}
# copy libcrypto
cp
-L
/usr/lib/x86_64-linux-gnu/libcrypto.so.1.1
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
cp
-L
/usr/lib/x86_64-linux-gnu/libcrypto.so.1.1
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
||
{
echo
"libcrypto.so.1.1 not found"
;
exit
1
;
}
# copy libSDL2
cp
-L
/usr/lib/x86_64-linux-gnu/libSDL2-2.0.so.0
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
cp
-L
/usr/lib/x86_64-linux-gnu/libSDL2-2.0.so.0
${
APPDIR
}
/usr/lib/x86_64-linux-gnu/
||
{
echo
"libSDL2-2.0.so.0 not found"
;
exit
1
;
}
# copy QGroundControl release into appimage
...
...
qgroundcontrol.pro
View file @
2430ec45
...
...
@@ -27,11 +27,12 @@ QGCROOT = $$PWD
DebugBuild
{
DESTDIR
=
$$
{
OUT_PWD
}
/
debug
#
DEFINES
+=
DEBUG
#
DEFINES
+=
ROS_BRIDGE_
CLIENT_
DEBUG
DEFINES
+=
DEBUG
#
DEFINES
+=
ROS_BRIDGE_DEBUG
}
else
{
DESTDIR
=
$$
{
OUT_PWD
}
/
release
#
DEFINES
+=
ROS_BRIDGE_DEBUG
DEFINES
+=
NDEBUG
}
...
...
@@ -438,13 +439,15 @@ HEADERS += \
src
/
Wima
/
Snake
/
PolygonArray
.
h
\
src
/
Wima
/
Snake
/
QNemoHeartbeat
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
QtROSJsonFactory
.
h
\
src
/
Wima
/
Snake
/
QtROSTypeFactory
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
SnakeTiles
.
h
\
src
/
Wima
/
Snake
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
Snake
/
SnakeWorker
.
h
\
src
/
Wima
/
WaypointManager
/
AreaInterface
.
h
\
src
/
Wima
/
WaypointManager
/
DefaultManager
.
h
\
src
/
Wima
/
WaypointManager
/
GenericWaypointManager
.
h
\
src
/
Wima
/
Geometry
/
WimaPolygonArray
.
h
\
src
/
Wima
/
Snake
/
snaketile
.
h
\
src
/
Wima
/
WaypointManager
/
RTLManager
.
h
\
src
/
Wima
/
WaypointManager
/
Settings
.
h
\
src
/
Wima
/
WaypointManager
/
Slicer
.
h
\
...
...
@@ -481,18 +484,21 @@ HEADERS += \
src
/
Wima
/
Geometry
/
testplanimetrycalculus
.
h
\
src
/
Settings
/
WimaSettings
.
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
/
ThreadSafeQueue
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TopicPublisher
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TopicSubscriber
.
h
\
src
/
comm
/
ros_bridge
/
include
/
TypeFactory
.
h
\
src
/
comm
/
ros_bridge
/
src
/
PackageBuffer
.
h
\
src
/
comm
/
ros_bridge
/
include
/
com_private
.
h
\
src
/
comm
/
ros_bridge
/
include
/
message_traits
.
h
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
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
SOURCES
+=
\
src
/
Snake
/
clipper
/
clipper
.
cpp
\
...
...
@@ -510,11 +516,21 @@ SOURCES += \
src
/
Wima
/
WaypointManager
/
Slicer
.
cpp
\
src
/
Wima
/
WaypointManager
/
Utils
.
cpp
\
src
/
Wima
/
WimaBridge
.
cc
\
src
/
comm
/
ros_bridge
/
include
/
ComPrivateInclude
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
MessageTag
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicPublisher
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicSubscriber
.
cpp
\
src
/
comm
/
ros_bridge
/
src
/
CasePacker
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
RosBridgeClient
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
com_private
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geographic_msgs
/
geopoint
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
messages
/
geometry_msgs
/
point32
.
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
/
api
/
QGCCorePlugin
.
cc
\
src
/
api
/
QGCOptions
.
cc
\
src
/
api
/
QGCSettings
.
cc
\
...
...
@@ -545,7 +561,7 @@ SOURCES += \
src
/
Wima
/
Geometry
/
testplanimetrycalculus
.
cpp
\
src
/
Settings
/
WimaSettings
.
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
)
...
...
src/Settings/Wima.SettingsGroup.json
View file @
2430ec45
...
...
@@ -18,5 +18,11 @@
"type"
:
"uint64"
,
"units"
:
"s"
,
"defaultValue"
:
15
},
{
"name"
:
"rosbridgeConnectionString"
,
"shortDescription"
:
"Ros Bridge Connection String (host:port)."
,
"type"
:
"string"
,
"defaultValue"
:
"localhost:9090"
}
]
src/Settings/WimaSettings.cc
View file @
2430ec45
...
...
@@ -11,3 +11,4 @@ DECLARE_SETTINGGROUP(Wima, "Wima")
DECLARE_SETTINGSFACT
(
WimaSettings
,
lowBatteryThreshold
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
enableLowBatteryHandling
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
minimalRemainingMissionTime
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
rosbridgeConnectionString
)
src/Settings/WimaSettings.h
View file @
2430ec45
...
...
@@ -13,4 +13,5 @@ public:
DEFINE_SETTINGFACT
(
lowBatteryThreshold
)
DEFINE_SETTINGFACT
(
enableLowBatteryHandling
)
DEFINE_SETTINGFACT
(
minimalRemainingMissionTime
)
DEFINE_SETTINGFACT
(
rosbridgeConnectionString
)
};
src/Wima/Geometry/GeoPoint3D.cpp
View file @
2430ec45
#include "GeoPoint3D.h"
GeoPoint
::
GeoPoint
(
QObject
*
parent
)
GeoPoint
3D
::
GeoPoint3D
(
QObject
*
parent
)
:
QObject
(
parent
),
ROSGeoPoint
()
{}
GeoPoint
::
GeoPoint
(
double
latitude
,
double
longitude
,
double
altitude
,
QObject
*
parent
)
GeoPoint
3D
::
GeoPoint3D
(
double
latitude
,
double
longitude
,
double
altitude
,
QObject
*
parent
)
:
QObject
(
parent
),
ROSGeoPoint
(
latitude
,
longitude
,
altitude
)
{}
GeoPoint
::
GeoPoint
(
const
GeoPoint
&
p
,
QObject
*
parent
)
GeoPoint
3D
::
GeoPoint3D
(
const
GeoPoint3D
&
p
,
QObject
*
parent
)
:
QObject
(
parent
),
ROSGeoPoint
(
p
.
latitude
(),
p
.
longitude
(),
p
.
altitude
())
{}
GeoPoint
::
GeoPoint
(
const
ROSGeoPoint
&
p
,
QObject
*
parent
)
GeoPoint
3D
::
GeoPoint3D
(
const
ROSGeoPoint
&
p
,
QObject
*
parent
)
:
QObject
(
parent
),
ROSGeoPoint
(
p
.
latitude
(),
p
.
longitude
(),
p
.
altitude
())
{}
GeoPoint
::
GeoPoint
(
const
QGeoCoordinate
&
p
,
QObject
*
parent
)
GeoPoint
3D
::
GeoPoint3D
(
const
QGeoCoordinate
&
p
,
QObject
*
parent
)
:
QObject
(
parent
),
ROSGeoPoint
(
p
.
latitude
(),
p
.
longitude
(),
p
.
altitude
())
{}
GeoPoint
*
GeoPoint
::
Clone
()
const
{
return
new
GeoPoint
(
*
this
);
}
GeoPoint
&
GeoPoint
::
operator
=
(
const
GeoPoint
&
p
)
GeoPoint3D
&
GeoPoint3D
::
operator
=
(
const
GeoPoint3D
&
p
)
{
this
->
setLatitude
(
p
.
latitude
());
this
->
setLongitude
(
p
.
longitude
());
...
...
@@ -32,7 +27,7 @@ GeoPoint &GeoPoint::operator=(const GeoPoint &p)
return
*
this
;
}
GeoPoint
&
GeoPoint
::
operator
=
(
const
QGeoCoordinate
&
p
)
GeoPoint
3D
&
GeoPoint3D
::
operator
=
(
const
QGeoCoordinate
&
p
)
{
this
->
setLatitude
(
p
.
latitude
());
this
->
setLongitude
(
p
.
longitude
());
...
...
src/Wima/Geometry/GeoPoint3D.h
View file @
2430ec45
#pragma once
#include "ros_bridge/include/JsonMethodes.h"
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/GenericMessages.h"
#include <QGeoCoordinate>
#include <QObject>
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
typedef
ROSBridge
::
MessageBaseClass
ROSMsg
;
typedef
ROSBridge
::
GenericMessages
::
GeographicMsgs
::
GeoPoint
ROSGeoPoint
;
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
class
GeoPoint
:
public
QObject
,
public
ROSGeoPoint
typedef
ros_bridge
::
messages
::
geographic_msgs
::
geo_point
::
GeoPoint
ROSGeoPoint
;
class
GeoPoint3D
:
public
QObject
,
public
ROSGeoPoint
{
Q_OBJECT
public:
typedef
MsgGroups
::
GeoPointGroup
Group
;
GeoPoint
(
QObject
*
parent
=
nullptr
);
GeoPoint
(
double
latitude
,
GeoPoint
3D
(
QObject
*
parent
=
nullptr
);
GeoPoint
3D
(
double
latitude
,
double
longitude
,
double
altitude
,
QObject
*
parent
=
nullptr
);
GeoPoint
(
const
GeoPoint
&
p
,
GeoPoint
3D
(
const
GeoPoint3D
&
p
,
QObject
*
parent
=
nullptr
);
GeoPoint
(
const
ROSGeoPoint
&
p
,
GeoPoint
3D
(
const
ROSGeoPoint
&
p
,
QObject
*
parent
=
nullptr
);
GeoPoint
(
const
QGeoCoordinate
&
p
,
GeoPoint
3D
(
const
QGeoCoordinate
&
p
,
QObject
*
parent
=
nullptr
);
virtual
GeoPoint
*
Clone
()
const
override
;
GeoPoint
&
operator
=
(
const
GeoPoint
&
p
);
GeoPoint
&
operator
=
(
const
QGeoCoordinate
&
p
);
GeoPoint3D
&
operator
=
(
const
GeoPoint3D
&
p
);
GeoPoint3D
&
operator
=
(
const
QGeoCoordinate
&
p
);
};
...
...
src/Wima/Geometry/Polygon2D.h
View file @
2430ec45
...
...
@@ -3,41 +3,30 @@
#include <QPolygonF>
#include <QPointF>
#include "ros_bridge/include/MessageGroups.h"
#include "ros_bridge/include/GenericMessages.h"
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
namespace
MsgGroupsNS
=
ROSBridge
::
MessageGroups
;
namespace
PolyStampedNS
=
ROSBridge
::
JsonMethodes
::
GeometryMsgs
::
PolygonStamped
;
typedef
ROSBridge
::
MessageBaseClass
ROSMsg
;
namespace
polygon_stamped
=
ros_bridge
::
messages
::
geometry_msgs
::
polygon_stamped
;
template
<
class
PointType
=
QPointF
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
>
class
Polygon2DTemplate
:
public
ROSMsg
{
//
typedef
ROSBridge
::
GenericMessages
::
GeometryMsgs
::
GenericPolygon
<
PointType
,
ContainerType
>
Poly
;
class
Polygon2DTemplate
{
//
typedef
ros_bridge
::
messages
::
geometry_msgs
::
polygon
::
GenericPolygon
<
PointType
,
ContainerType
>
Polygon
;
public:
typedef
MsgGroupsNS
::
PolygonStampedGroup
Group
;
// has no header
Polygon2DTemplate
(){}
Polygon2DTemplate
(
const
Polygon2DTemplate
&
other
)
:
ROSMsg
(),
_polygon
(
other
.
_polygon
){}
Polygon2DTemplate
(
const
Polygon2DTemplate
&
other
)
:
_polygon
(
other
.
_polygon
){}
Polygon2DTemplate
&
operator
=
(
const
Polygon2DTemplate
&
other
)
{
this
->
_polygon
=
other
.
_polygon
;
return
*
this
;
}
const
Poly
&
polygon
()
const
{
return
_polygon
;}
Poly
&
polygon
()
{
return
_polygon
;}
const
Poly
gon
&
polygon
()
const
{
return
_polygon
;}
Poly
gon
&
polygon
()
{
return
_polygon
;}
const
ContainerType
<
PointType
>
&
path
()
const
{
return
_polygon
.
points
();}
ContainerType
<
PointType
>
&
path
()
{
return
_polygon
.
points
();}
virtual
Polygon2DTemplate
*
Clone
()
const
{
// ROSMsg
return
new
Polygon2DTemplate
(
*
this
);
}
private:
Poly
_polygon
;
Poly
gon
_polygon
;
};
...
...
src/Wima/Geometry/PolygonArray.h
View file @
2430ec45
#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
>
class
PolygonArray
:
public
ROSMsgBase
,
public
ContainerType
<
PolygonType
>
{
class
PolygonArray
:
public
ContainerType
<
PolygonType
>
{
public:
explicit
PolygonArray
()
:
ROSMsgBase
(),
ContainerType
<
PolygonType
>
()
{}
explicit
PolygonArray
()
:
ContainerType
<
PolygonType
>
()
{}
PolygonArray
(
const
PolygonArray
&
other
)
:
ContainerType
<
PolygonType
>
(
other
)
{}
QString
type
()
const
override
{
return
"PolygonArray"
;}
...
...
src/Wima/Geometry/WimaPolygonArray.h
View file @
2430ec45
#pragma once
#include "ros_bridge/include/MessageBaseClass.h"
#include "ros_bridge/include/MessageGroups.h"
#include "QmlObjectListModel.h"
#include <QVector>
#include <QString>
typedef
ROSBridge
::
MessageBaseClass
ROSMsg
;
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
typedef
MsgGroups
::
EmptyGroup
EmptyGroup
;
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
,
typename
GroupType
=
EmptyGroup
>
class
WimaPolygonArray
:
public
ROSMsg
template
<
class
PolygonType
,
template
<
class
,
class
...
>
class
ContainerType
=
QVector
>
class
WimaPolygonArray
{
public:
typedef
GroupType
Group
;
WimaPolygonArray
()
{}
WimaPolygonArray
(
const
WimaPolygonArray
&
other
)
:
ROSMsg
()
,
_polygons
(
other
.
_polygons
),
_dirty
(
true
)
_polygons
(
other
.
_polygons
),
_dirty
(
true
)
{}
virtual
WimaPolygonArray
*
Clone
()
const
override
{
return
new
WimaPolygonArray
(
*
this
);
~
WimaPolygonArray
(){
_objs
.
clearAndDeleteContents
();
}
class
QmlObjectListModel
*
QmlObjectListModel
(){
...
...
@@ -51,9 +43,9 @@ public:
private:
void
_updateObjects
(
void
){
_objs
.
clear
();
_objs
.
clear
AndDeleteContents
();
for
(
long
i
=
0
;
i
<
_polygons
.
size
();
++
i
){
_objs
.
append
(
&
_polygons
[
i
]
);
_objs
.
append
(
new
PolygonType
(
_polygons
[
i
])
);
}
}
...
...
src/Wima/Snake/PolygonArray.h
View file @
2430ec45
#pragma once
#include "ros_bridge/include/MessageGroups.h"
#include "Wima/Geometry/Polygon2D.h"
#include "Wima/Geometry/WimaPolygonArray.h"
namespace
MsgGroups
=
ROSBridge
::
MessageGroups
;
typedef
WimaPolygonArray
<
Polygon2D
,
QVector
,
MsgGroups
::
PolygonArrayGroup
>
PolygonArray
;
typedef
WimaPolygonArray
<
Polygon2D
,
QVector
>
SnakeTilesLocal
;
src/Wima/Snake/QNemoHeartbeat.h
View file @
2430ec45
#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 @
2430ec45
#pragma once
#include <QVector>
#include <QObject>
#include "ros_bridge/include/GenericMessages.h"
namespace
NemoMsgs
=
ROSBridge
::
GenericMessages
::
NemoMsgs
;
typedef
NemoMsgs
::
GenericProgress
<
int
,
QVector
>
QNemoProgress
;
#include <QVector>
#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 @
0e34b591
#pragma once
#include "ros_bridge/include/JsonFactory.h"
#include <QString>
typedef
ROSBridge
::
GenericJsonFactory
<>
QtROSJsonFactory
;
src/Wima/Snake/QtROSTypeFactory.h
deleted
100644 → 0
View file @
0e34b591
#pragma once
#include "ros_bridge/include/TypeFactory.h"
#include <QString>
typedef
ROSBridge
::
TypeFactory
QtROSTypeFactory
;
src/Wima/WimaController.cc
View file @
2430ec45
#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 "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/QNemoHeartbeat.h"
#include "QVector3D"
#include <QScopedPointer>
#include <memory>
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
// const char* WimaController::wimaFileExtension = "wima";
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
const
char
*
WimaController
::
missionItemsName
=
"MissionItems"
;
const
char
*
WimaController
::
settingsGroup
=
"WimaController"
;
...
...
@@ -57,7 +64,8 @@ WimaController::WimaController(QObject *parent)
,
_rtlManager
(
_managerSettings
,
_areaInterface
)
,
_currentManager
(
&
_defaultManager
)
,
_managerList
{
&
_defaultManager
,
&
_snakeManager
,
&
_rtlManager
}
,
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
,
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/WimaController.SettingsGroup.json"
),
this
))
,
_enableWimaController
(
settingsGroup
,
_metaDataMap
[
enableWimaControllerName
])
,
_overlapWaypoints
(
settingsGroup
,
_metaDataMap
[
overlapWaypointsName
])
...
...
@@ -68,19 +76,37 @@ WimaController::WimaController(QObject *parent)
,
_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
])
,
_lowBatteryHandlingTriggered
(
false
)
,
_measurementPathLength
(
-
1
)
,
_snakeCalcInProgress
(
false
)
,
_nemoHeartbeat
(
0
/*status: not connected*/
)
,
_fallbackStatus
(
0
/*status: not connected*/
)
,
_bridgeSetupDone
(
false
)
,
_pRosBridge
(
new
ROSBridge
::
ROSBridge
())
,
_batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
1000
/*ms*/
)
,
_snakeTicker
(
EVENT_TIMER_INTERVAL
,
200
/*ms*/
)
,
_nemoTimeoutTicker
(
EVENT_TIMER_INTERVAL
,
5000
/*ms*/
)
,
_topicServiceSetupDone
(
false
)
{
// ROS Bridge.
WimaSettings
*
wimaSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
();
auto
connectionStringFact
=
wimaSettings
->
rosbridgeConnectionString
();
auto
setConnectionString
=
[
connectionStringFact
,
this
]{
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
if
(
ros_bridge
::
isValidConnectionString
(
connectionString
.
toLocal8Bit
().
data
())
){
this
->
_pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
}
else
{
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
connectionString
);
this
->
_pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
"localhost:9090"
));
}
};
setConnectionString
();
connect
(
wimaSettings
->
rosbridgeConnectionString
(),
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
// Set up facts.
_showAllMissionItems
.
setRawValue
(
true
);
_showCurrentMissionItems
.
setRawValue
(
true
);
...
...
@@ -112,11 +138,13 @@ WimaController::WimaController(QObject *parent)
_eventTimer
.
start
(
EVENT_TIMER_INTERVAL
);
// Snake Worker Thread.
connect
(
&
_snakeWorker
,
&
Snake
DataManager
::
finished
,
this
,
&
WimaController
::
_updateSnakeData
);
connect
(
&
_snakeWorker
,
&
Snake
Worker
::
finished
,
this
,
&
WimaController
::
_snakeStoreWorkerResults
);
connect
(
this
,
&
WimaController
::
nemoProgressChanged
,
this
,
&
WimaController
::
_initStartSnakeWorker
);
connect
(
this
,
&
QObject
::
destroyed
,
&
this
->
_snakeWorker
,
&
Snake
DataManag
er
::
quit
);
connect
(
this
,
&
QObject
::
destroyed
,
&
this
->
_snakeWorker
,
&
Snake
Work
er
::
quit
);
// Snake.
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_initStartSnakeWorker
);
_initStartSnakeWorker
();
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_switchSnakeManager
);
_switchSnakeManager
(
_enableSnake
.
rawValue
());
}
...
...
@@ -359,6 +387,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
// reset visual items
_areas
.
clear
();
_defaultManager
.
clear
();
_snakeManager
.
clear
();
_snakeTiles
.
polygons
().
clear
();
_snakeTilesLocal
.
polygons
().
clear
();
_snakeTileCenterPoints
.
clear
();
...
...
@@ -449,10 +478,106 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
_localPlanDataValid
=
true
;
_initStartSnakeWorker
();
// Initialize _scenario.
Area
mArea
;
for
(
auto
variant
:
_measurementArea
.
path
()){
QGeoCoordinate
c
{
variant
.
value
<
QGeoCoordinate
>
()};
mArea
.
geoPolygon
.
push_back
(
GeoPoint2D
{
c
.
latitude
(),
c
.
longitude
()});
}
mArea
.
type
=
AreaType
::
MeasurementArea
;
Area
sArea
;
for
(
auto
variant
:
_serviceArea
.
path
()){
QGeoCoordinate
c
{
variant
.
value
<
QGeoCoordinate
>
()};
sArea
.
geoPolygon
.
push_back
(
GeoPoint2D
{
c
.
latitude
(),
c
.
longitude
()});
}
sArea
.
type
=
AreaType
::
ServiceArea
;
Area
corridor
;
for
(
auto
variant
:
_corridor
.
path
()){
QGeoCoordinate
c
{
variant
.
value
<
QGeoCoordinate
>
()};
corridor
.
geoPolygon
.
push_back
(
GeoPoint2D
{
c
.
latitude
(),
c
.
longitude
()});
}
corridor
.
type
=
AreaType
::
Corridor
;
_scenario
.
addArea
(
mArea
);
_scenario
.
addArea
(
sArea
);
_scenario
.
addArea
(
corridor
);
// Check if scenario is defined.
if
(
!
_scenario
.
defined
(
_snakeTileWidth
.
rawValue
().
toUInt
(),
_snakeTileHeight
.
rawValue
().
toUInt
(),
_snakeMinTileArea
.
rawValue
().
toUInt
())
)
{
Q_ASSERT
(
false
);
return
false
;
}
{
// Get tiles and origin.
auto
origin
=
_scenario
.
getOrigin
();
_snakeOrigin
.
setLatitude
(
origin
[
0
]);
_snakeOrigin
.
setLongitude
(
origin
[
1
]);
_snakeOrigin
.
setAltitude
(
origin
[
2
]);
const
auto
&
tiles
=
_scenario
.
getTiles
();
const
auto
&
cps
=
_scenario
.
getTileCenterPoints
();
for
(
unsigned
int
i
=
0
;
i
<
tiles
.
size
();
++
i
)
{
const
auto
&
tile
=
tiles
[
i
];
SnakeTile
Tile
;
for
(
const
auto
&
vertex
:
tile
)
{
QGeoCoordinate
QVertex
(
vertex
[
0
],
vertex
[
1
],
vertex
[
2
]);
Tile
.
append
(
QVertex
);
}
const
auto
&
centerPoint
=
cps
[
i
];
QGeoCoordinate
QCenterPoint
(
centerPoint
[
0
],
centerPoint
[
1
],
centerPoint
[
2
]);
Tile
.
setCenter
(
QCenterPoint
);
_snakeTiles
.
polygons
().
append
(
Tile
);
_snakeTileCenterPoints
.
append
(
QVariant
::
fromValue
(
QCenterPoint
));
}
}
{
// Get local tiles.
const
auto
&
tiles
=
_scenario
.
getTilesENU
();
for
(
unsigned
int
i
=
0
;
i
<
tiles
.
size
();
++
i
)
{
const
auto
&
tile
=
tiles
[
i
];
Polygon2D
Tile
;
for
(
const
auto
&
vertex
:
tile
.
outer
())
{
QPointF
QVertex
(
vertex
.
get
<
0
>
(),
vertex
.
get
<
1
>
());
Tile
.
path
().
append
(
QVertex
);
}
_snakeTilesLocal
.
polygons
().
append
(
Tile
);
}
}
emit
snakeTilesChanged
();
emit
snakeTileCenterPointsChanged
();
if
(
_enableSnake
.
rawValue
().
toBool
()
&&
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
&&
_topicServiceSetupDone
&&
_snakeTilesLocal
.
polygons
().
size
()
>
0
)
{
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
;
return
true
;
}
...
...
@@ -624,13 +749,9 @@ void WimaController::_checkBatteryLevel()
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
()
)
if
(
enableLowBatteryHandling
->
rawValue
().
toBool
()
&&
_
batteryLevelTicker
.
ready
()
)
_checkBatteryLevel
();
// Snake flight plan update necessary?
...
...
@@ -639,77 +760,31 @@ void WimaController::_eventTimerHandler()
// }
// }
if
(
nemoStatusTicker
.
ready
()
)
{
if
(
_nemoTimeoutTicker
.
ready
()
&&
_enableSnake
.
rawValue
().
toBool
()
)
{
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
();
if
(
_snakeTicker
.
ready
()
)
{
if
(
_enableSnake
.
rawValue
().
toBool
()
)
{
if
(
!
_pRosBridge
->
isRunning
())
{
_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
(
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
&&
!
_topicServiceSetupDone
)
{
if
(
_doTopicServiceSetup
()
)
_topicServiceSetupDone
=
true
;
}
else
if
(
_pRosBridge
->
isRunning
()
&&
!
_pRosBridge
->
connected
()
&&
_topicServiceSetupDone
){
_pRosBridge
->
reset
();
_pRosBridge
->
start
();
_topicServiceSetupDone
=
false
;
}
}
else
if
(
_bridgeSetupDone
)
{
}
else
if
(
_pRosBridge
->
isRunning
()
)
{
_pRosBridge
->
reset
();
_bridg
eSetupDone
=
false
;
_topicServic
eSetupDone
=
false
;
}
}
}
...
...
@@ -818,12 +893,13 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress)
}
}
void
WimaController
::
_
updateSnakeData
()
void
WimaController
::
_
snakeStoreWorkerResults
()
{
_setSnakeCalcInProgress
(
false
);
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_snakeManager
.
clear
();
const
auto
&
r
=
_snakeWorker
.
result
();
const
auto
&
r
=
_snakeWorker
.
getResult
();
if
(
!
r
.
success
)
{
//qgcApp()->showMessage(r.errorMessage);
return
;
...
...
@@ -839,44 +915,19 @@ void WimaController::_updateSnakeData()
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
)]);
_snakeManager
.
push_back
(
r
.
waypoints
[
int
(
i
)]
.
value
<
QGeoCoordinate
>
()
);
}
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."
;
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
double
duration
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
qWarning
()
<<
"WimaController::_snakeStoreWorkerResults execution time: "
<<
duration
<<
" ms."
;
}
void
WimaController
::
_initStartSnakeWorker
()
...
...
@@ -890,12 +941,6 @@ void WimaController::_initStartSnakeWorker()
}
// Initialize _snakeWorker.
_snakeWorker
.
setMeasurementArea
(
_measurementArea
.
coordinateList
());
_snakeWorker
.
setServiceArea
(
_serviceArea
.
coordinateList
());
_snakeWorker
.
setCorridor
(
_corridor
.
coordinateList
());
_snakeWorker
.
setProgress
(
_nemoProgress
.
progress
());
_snakeWorker
.
setLineDistance
(
...
...
@@ -922,3 +967,115 @@ void WimaController::_switchSnakeManager(QVariant variant)
_switchWaypointManager
(
_defaultManager
);
}
}
bool
WimaController
::
_doTopicServiceSetup
()
{
using
namespace
ros_bridge
::
messages
;
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"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
int
requiredSize
=
this
->
_snakeTilesLocal
.
polygons
().
size
();
auto
&
progress_msg
=
this
->
_nemoProgress
;
if
(
!
nemo_msgs
::
progress
::
fromJson
(
*
pDoc
,
progress_msg
)
||
progress_msg
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
// Set progress to default.
progress_msg
.
progress
().
fill
(
0
,
requiredSize
);
// Publish snake origin.
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
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
();
});
// Subscribe /nemo/heartbeat.
_pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
auto
&
heartbeat_msg
=
this
->
_nemoHeartbeat
;
if
(
!
nemo_msgs
::
heartbeat
::
fromJson
(
*
pDoc
,
heartbeat_msg
)
)
{
if
(
heartbeat_msg
.
status
()
==
this
->
_fallbackStatus
)
return
;
heartbeat_msg
.
setStatus
(
this
->
_fallbackStatus
);
}
this
->
_nemoTimeoutTicker
.
reset
();
this
->
_fallbackStatus
=
-
1
;
/*Timeout*/
emit
WimaController
::
nemoStatusChanged
();
emit
WimaController
::
nemoStatusStringChanged
();
});
// 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"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
JsonDocUPtr
pDoc
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
rapidjson
::
Value
jSnakeTiles
(
rapidjson
::
kObjectType
);
bool
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
_snakeTilesLocal
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
return
pDoc
;
});
return
true
;
}
src/Wima/WimaController.h
View file @
2430ec45
...
...
@@ -35,18 +35,14 @@
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "ros_bridge/include/
ROSB
ridge.h"
#include "ros_bridge/include/
ros_b
ridge.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"
#include
<map>
#include
"utilities.h"
#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
#include <map>
using
namespace
snake
;
...
...
@@ -59,7 +55,7 @@ class WimaController : public QObject
enum
FileType
{
WimaFile
,
PlanFile
};
typedef
QScopedPointer
<
ROSB
ridge
::
ROSBridge
>
ROSBridgePtr
;
typedef
QScopedPointer
<
ros_b
ridge
::
ROSBridge
>
ROSBridgePtr
;
public:
...
...
@@ -338,9 +334,10 @@ private slots:
void
_smartRTLCleanUp
(
bool
flying
);
// Snake.
void
_setSnakeCalcInProgress
(
bool
inProgress
);
void
_
updateSnakeData
();
void
_
snakeStoreWorkerResults
();
void
_initStartSnakeWorker
();
void
_switchSnakeManager
(
QVariant
variant
);
bool
_doTopicServiceSetup
();
// Periodic tasks.
void
_eventTimerHandler
(
void
);
// Waypoint manager handling.
...
...
@@ -396,8 +393,6 @@ private:
// 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
;
...
...
@@ -410,11 +405,14 @@ private:
QNemoHeartbeat
_nemoHeartbeat
;
// measurement progress
int
_fallbackStatus
;
ROSBridgePtr
_pRosBridge
;
bool
_bridgeSetupDone
;
static
StatusMap
_nemoStatusMap
;
bool
_topicServiceSetupDone
;
// Periodic tasks.
QTimer
_eventTimer
;
EventTicker
_batteryLevelTicker
;
EventTicker
_snakeTicker
;
EventTicker
_nemoTimeoutTicker
;
};
...
...
src/comm/ros_bridge/include/CasePacker.h
deleted
100644 → 0
View file @
0e34b591
#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 @
0e34b591
#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 @
0e34b591
#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
(){}
GenericProgress
(
const
ContainterType
<
IntType
>
&
progress
)
:
_progress
(
progress
){}
GenericProgress
(
const
GenericProgress
&
p
)
:
_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 @
0e34b591
#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 @
0e34b591
#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 @
0e34b591
#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 @
0e34b591
#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 @
0e34b591
#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 @
0e34b591
#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
0 → 100644
View file @
2430ec45
#include "RosBridgeClient.h"
#include <chrono>
#include <functional>
#include <thread>
#include <future>
#include <regex>
struct
Task
{
std
::
function
<
bool
(
void
)
>
ready
;
// Condition under which command should be executed.
std
::
function
<
void
(
void
)
>
execute
;
// Command to execute.
std
::
function
<
bool
(
void
)
>
expired
;
// Returns true if the command is expired.
std
::
function
<
void
(
void
)
>
clear_up
;
// operation to perform if task expired.
std
::
string
name
;
};
void
RosbridgeWsClient
::
start
(
const
std
::
__cxx11
::
string
&
client_name
,
std
::
shared_ptr
<
WsClient
>
client
,
const
std
::
__cxx11
::
string
&
message
)
{
#ifndef ROS_BRIDGE_DEBUG
(
void
)
client_name
;
#endif
if
(
!
client
->
on_open
)
{
#ifdef ROS_BRIDGE_DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
};
}
#ifdef ROS_BRIDGE_DEBUG
if
(
!
client
->
on_message
)
{
client
->
on_message
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
std
::
cout
<<
client_name
<<
": Message received: "
<<
in_message
->
string
()
<<
std
::
endl
;
};
}
if
(
!
client
->
on_close
)
{
client
->
on_close
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
int
status
,
const
std
::
string
&
/*reason*/
)
{
std
::
cout
<<
client_name
<<
": Closed connection with status code "
<<
status
<<
std
::
endl
;
};
}
if
(
!
client
->
on_error
)
{
// See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings
client
->
on_error
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
const
SimpleWeb
::
error_code
&
ec
)
{
std
::
cout
<<
client_name
<<
": Error: "
<<
ec
<<
", error message: "
<<
ec
.
message
()
<<
std
::
endl
;
};
}
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
thread
client_thread
([
client_name
,
client
]()
{
#else
std
::
thread
client_thread
([
client
]()
{
#endif
client
->
start
();
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Terminated"
<<
std
::
endl
;
#endif
client
->
on_open
=
NULL
;
client
->
on_message
=
NULL
;
client
->
on_close
=
NULL
;
client
->
on_error
=
NULL
;
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
" thread end"
<<
std
::
endl
;
#endif
});
client_thread
.
detach
();
}
RosbridgeWsClient
::
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
,
bool
run
)
:
server_port_path
(
server_port_path
)
,
isConnected
(
std
::
make_shared
<
std
::
atomic_bool
>
(
false
))
,
stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
if
(
run
)
this
->
run
();
}
RosbridgeWsClient
::
RosbridgeWsClient
(
const
std
::
__cxx11
::
string
&
server_port_path
)
:
RosbridgeWsClient
::
RosbridgeWsClient
(
server_port_path
,
true
)
{
}
RosbridgeWsClient
::~
RosbridgeWsClient
()
{
reset
();
}
bool
RosbridgeWsClient
::
connected
(){
return
isConnected
->
load
();
}
void
RosbridgeWsClient
::
run
()
{
if
(
!
stopped
->
load
()
)
return
;
stopped
->
store
(
false
);
// Start periodic thread to monitor connection status, advertised topics etc.
periodic_thread
=
std
::
make_shared
<
std
::
thread
>
([
this
]
{
std
::
list
<
Task
>
task_list
;
constexpr
auto
poll_interval
=
std
::
chrono
::
seconds
(
1
);
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
();
while
(
!
this
->
stopped
->
load
()
)
{
// ====================================================================================
// Add tasks.
// ====================================================================================
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
)
{
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
std
::
string
reset_status_task_name
=
"reset_status_task"
;
// Add status task if necessary.
auto
const
it
=
std
::
find_if
(
task_list
.
begin
(),
task_list
.
end
(),
[
&
reset_status_task_name
](
const
Task
&
t
){
return
t
.
name
==
reset_status_task_name
;
});
if
(
it
==
task_list
.
end
()
){
// Check connection status.
auto
status_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
auto
status_client
=
std
::
make_shared
<
WsClient
>
(
this
->
server_port_path
);
status_client
->
on_open
=
[
status_set
,
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
)
{
this
->
isConnected
->
store
(
true
);
status_set
->
store
(
true
);
};
std
::
thread
status_thread
([
status_client
]{
status_client
->
start
();
status_client
->
on_open
=
NULL
;
status_client
->
on_message
=
NULL
;
status_client
->
on_close
=
NULL
;
status_client
->
on_error
=
NULL
;
});
status_thread
.
detach
();
// Create task to reset isConnected after one second.
Task
reset_task
;
reset_task
.
name
=
reset_status_task_name
;
// condition
auto
now
=
std
::
chrono
::
high_resolution_clock
::
now
();
const
auto
t_trigger
=
now
+
std
::
chrono
::
seconds
(
1
);
reset_task
.
ready
=
[
t_trigger
]{
return
std
::
chrono
::
high_resolution_clock
::
now
()
>
t_trigger
;
};
// command
reset_task
.
execute
=
[
status_client
,
status_set
,
this
]{
status_client
->
stop
();
this
->
isConnected
->
store
(
false
);
status_set
->
store
(
true
);
};
// expired
reset_task
.
expired
=
[
status_set
]{
return
status_set
->
load
();
};
// clear up
reset_task
.
clear_up
=
[
status_client
,
this
]{
status_client
->
stop
();
};
task_list
.
push_back
(
reset_task
);
}
if
(
this
->
isConnected
->
load
()
){
// Add available topics task if neccessary.
std
::
string
reset_topics_task_name
=
"reset_topics_task"
;
auto
const
topics_it
=
std
::
find_if
(
task_list
.
begin
(),
task_list
.
end
(),
[
&
reset_topics_task_name
](
const
Task
&
t
){
return
t
.
name
==
reset_topics_task_name
;
});
if
(
topics_it
==
task_list
.
end
()
){
// Call /rosapi/topics service.
auto
topics_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
this
->
callService
(
"/rosapi/topics"
,
[
topics_set
,
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_topics
=
in_message
->
string
();
lk
.
unlock
();
topics_set
->
store
(
true
);
connection
->
send_close
(
1000
);
});
// Create task to reset topics after one second.
Task
reset_task
;
reset_task
.
name
=
reset_topics_task_name
;
// condition
auto
now
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
t_trigger
=
now
+
std
::
chrono
::
seconds
(
1
);
reset_task
.
ready
=
[
t_trigger
]{
return
std
::
chrono
::
high_resolution_clock
::
now
()
>
t_trigger
;
};
// command
reset_task
.
execute
=
[
topics_set
,
this
]{
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_topics
.
clear
();
lk
.
unlock
();
topics_set
->
store
(
true
);
};
// expired
reset_task
.
expired
=
[
topics_set
]{
return
topics_set
->
load
();
};
// clear up
reset_task
.
clear_up
=
[
this
]{
return
;
};
task_list
.
push_back
(
reset_task
);
}
// Add available services task if neccessary.
std
::
string
reset_services_name
=
"reset_services_task"
;
auto
const
services_it
=
std
::
find_if
(
task_list
.
begin
(),
task_list
.
end
(),
[
&
reset_services_name
](
const
Task
&
t
){
return
t
.
name
==
reset_services_name
;
});
if
(
services_it
==
task_list
.
end
()
){
// Call /rosapi/services service.
auto
services_set
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
this
->
callService
(
"/rosapi/services"
,
[
this
,
services_set
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_services
=
in_message
->
string
();
lk
.
unlock
();
services_set
->
store
(
true
);
connection
->
send_close
(
1000
);
});
// Create task to reset services after one second.
Task
reset_task
;
reset_task
.
name
=
reset_services_name
;
// condition
auto
now
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
t_trigger
=
now
+
std
::
chrono
::
seconds
(
1
);
reset_task
.
ready
=
[
t_trigger
]{
return
std
::
chrono
::
high_resolution_clock
::
now
()
>
t_trigger
;
};
// command
reset_task
.
execute
=
[
services_set
,
this
]{
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_services
.
clear
();
lk
.
unlock
();
services_set
->
store
(
true
);
};
// expired
reset_task
.
expired
=
[
services_set
]{
return
services_set
->
load
();
};
// clear up
reset_task
.
clear_up
=
[
this
]{
return
;
};
task_list
.
push_back
(
reset_task
);
}
}
else
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
available_topics
.
clear
();
available_services
.
clear
();
}
}
// ====================================================================================
// Process tasks.
// ====================================================================================
for
(
auto
task_it
=
task_list
.
begin
();
task_it
!=
task_list
.
end
();
){
if
(
!
task_it
->
expired
()
){
if
(
task_it
->
ready
()
){
task_it
->
execute
();
task_it
=
task_list
.
erase
(
task_it
);
}
else
{
++
task_it
;
}
}
else
{
task_it
->
clear_up
();
task_it
=
task_list
.
erase
(
task_it
);
}
}
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
10
));
}
// Clear up remaining tasks.
for
(
auto
task_it
=
task_list
.
begin
();
task_it
!=
task_list
.
end
();
++
task_it
){
task_it
->
clear_up
();
}
task_list
.
clear
();
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
"periodic thread end"
<<
std
::
endl
;
#endif
});
}
void
RosbridgeWsClient
::
stop
()
{
if
(
stopped
->
load
()
)
return
;
stopped
->
store
(
true
);
periodic_thread
->
join
();
}
void
RosbridgeWsClient
::
reset
()
{
stop
();
unsubscribeAll
();
unadvertiseAll
();
unadvertiseAllServices
();
std
::
unique_lock
<
std
::
mutex
>
lk
(
mutex
);
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
();
}
}
void
RosbridgeWsClient
::
addClient
(
const
std
::
string
&
client_name
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
==
client_map
.
end
())
{
client_map
[
client_name
]
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std
::
cerr
<<
client_name
<<
" has already been created"
<<
std
::
endl
;
}
#endif
}
std
::
shared_ptr
<
WsClient
>
RosbridgeWsClient
::
getClient
(
const
std
::
string
&
client_name
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
return
it
->
second
;
}
return
NULL
;
}
void
RosbridgeWsClient
::
stopClient
(
const
std
::
string
&
client_name
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving the client have been launched.
std
::
shared_ptr
<
WsClient
>
client
=
it
->
second
;
#ifdef ROS_BRIDGE_DEBUG
std
::
thread
t
([
client
,
client_name
](){
#else
std
::
thread
t
([
client
](){
#endif
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
100
));
client
->
stop
();
// The next lines of code seem to cause a double free or corruption error, why?
// client->on_open = NULL;
// client->on_message = NULL;
// client->on_close = NULL;
// client->on_error = NULL;
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
" has been removed"
<<
std
::
endl
;
#endif
});
t
.
detach
();
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
}
#endif
}
void
RosbridgeWsClient
::
removeClient
(
const
std
::
string
&
client_name
)
{
stopClient
(
client_name
);
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
client_map
.
erase
(
it
);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
}
#endif
}
}
std
::
string
RosbridgeWsClient
::
getAdvertisedTopics
(){
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
return
available_topics
;
}
std
::
string
RosbridgeWsClient
::
getAdvertisedServices
(){
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
return
available_services
;
}
bool
RosbridgeWsClient
::
topicAvailable
(
const
std
::
string
&
topic
){
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
"checking if topic "
<<
topic
<<
" is available"
<<
std
::
endl
;
std
::
cout
<<
"available topics: "
<<
available_topics
<<
std
::
endl
;
#endif
size_t
pos
;
{
pos
=
available_topics
.
find
(
topic
);
}
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
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it_client
=
client_map
.
find
(
client_name
);
if
(
it_client
!=
client_map
.
end
())
{
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
[
topic
](
const
EntryData
&
td
){
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
if
(
it_ser_top
!=
service_topic_list
.
end
()){
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
#endif
return
;
}
auto
client
=
it_client
->
second
;
std
::
weak_ptr
<
WsClient
>
wpClient
=
client
;
service_topic_list
.
push_back
(
std
::
make_tuple
(
EntryType
::
AdvertisedTopic
,
topic
,
client_name
,
wpClient
));
std
::
string
message
=
"
\"
op
\"
:
\"
advertise
\"
,
\"
topic
\"
:
\"
"
+
topic
+
"
\"
,
\"
type
\"
:
\"
"
+
type
+
"
\"
"
;
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
#ifdef ROS_BRIDGE_DEBUG
client
->
on_open
=
[
this
,
topic
,
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
client
->
on_open
=
[
this
,
topic
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
};
start
(
client_name
,
client
,
message
);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
}
#endif
}
void
RosbridgeWsClient
::
unadvertise
(
const
std
::
string
&
topic
,
const
std
::
string
&
id
){
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
[
&
topic
](
const
EntryData
&
td
){
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
if
(
it_ser_top
==
service_topic_list
.
end
()){
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" not advertised"
<<
std
::
endl
;
#endif
return
;
}
std
::
string
message
=
"
\"
op
\"
:
\"
unadvertise
\"
"
;
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
message
+=
",
\"
topic
\"
:
\"
"
+
topic
+
"
\"
"
;
message
=
"{"
+
message
+
"}"
;
std
::
string
client_name
=
"topic_unadvertiser"
+
topic
;
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef ROS_BRIDGE_DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
// unadvertise
connection
->
send_close
(
1000
);
};
start
(
client_name
,
client
,
message
);
service_topic_list
.
erase
(
it_ser_top
);
}
void
RosbridgeWsClient
::
unadvertiseAll
(){
for
(
auto
entry
:
service_topic_list
){
if
(
std
::
get
<
integral
(
EntryEnum
::
EntryType
)
>
(
entry
)
==
EntryType
::
AdvertisedTopic
){
unadvertise
(
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
entry
));
}
}
}
void
RosbridgeWsClient
::
publish
(
const
std
::
string
&
topic
,
const
rapidjson
::
Document
&
msg
,
const
std
::
string
&
id
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
[
&
topic
](
const
EntryData
&
td
){
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
if
(
it_ser_top
==
service_topic_list
.
end
()
){
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" not yet advertised"
<<
std
::
endl
;
#endif
return
;
}
rapidjson
::
StringBuffer
strbuf
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
strbuf
);
msg
.
Accept
(
writer
);
std
::
string
client_name
=
"publish_client"
+
topic
;
std
::
string
message
=
"
\"
op
\"
:
\"
publish
\"
,
\"
topic
\"
:
\"
"
+
topic
+
"
\"
,
\"
msg
\"
:"
+
strbuf
.
GetString
();
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
std
::
shared_ptr
<
WsClient
>
publish_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef ROS_BRIDGE_DEBUG
publish_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
publish_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message."
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
// TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
connection
->
send_close
(
1000
);
};
start
(
client_name
,
publish_client
,
message
);
}
void
RosbridgeWsClient
::
subscribe
(
const
std
::
string
&
client_name
,
const
std
::
string
&
topic
,
const
InMessage
&
callback
,
const
std
::
string
&
id
,
const
std
::
string
&
type
,
int
throttle_rate
,
int
queue_length
,
int
fragment_size
,
const
std
::
string
&
compression
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it_client
=
client_map
.
find
(
client_name
);
if
(
it_client
!=
client_map
.
end
())
{
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
[
topic
](
const
EntryData
&
td
){
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
if
(
it_ser_top
!=
service_topic_list
.
end
()){
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
#endif
return
;
}
auto
client
=
it_client
->
second
;
std
::
weak_ptr
<
WsClient
>
wpClient
=
client
;
service_topic_list
.
push_back
(
std
::
make_tuple
(
EntryType
::
SubscribedTopic
,
topic
,
client_name
,
wpClient
));
std
::
string
message
=
"
\"
op
\"
:
\"
subscribe
\"
,
\"
topic
\"
:
\"
"
+
topic
+
"
\"
"
;
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
if
(
type
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
type
\"
:
\"
"
+
type
+
"
\"
"
;
}
if
(
throttle_rate
>
-
1
)
{
message
+=
",
\"
throttle_rate
\"
:"
+
std
::
to_string
(
throttle_rate
);
}
if
(
queue_length
>
-
1
)
{
message
+=
",
\"
queue_length
\"
:"
+
std
::
to_string
(
queue_length
);
}
if
(
fragment_size
>
-
1
)
{
message
+=
",
\"
fragment_size
\"
:"
+
std
::
to_string
(
fragment_size
);
}
if
(
compression
.
compare
(
"none"
)
==
0
||
compression
.
compare
(
"png"
)
==
0
)
{
message
+=
",
\"
compression
\"
:
\"
"
+
compression
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
client
->
on_message
=
callback
;
this
->
start
(
client_name
,
client
,
message
);
// subscribe to topic
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
}
#endif
}
void
RosbridgeWsClient
::
unsubscribe
(
const
std
::
string
&
topic
,
const
std
::
string
&
id
){
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
[
topic
](
const
EntryData
&
td
){
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
if
(
it_ser_top
==
service_topic_list
.
end
()){
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"topic: "
<<
topic
<<
" not advertised"
<<
std
::
endl
;
#endif
return
;
}
std
::
string
message
=
"
\"
op
\"
:
\"
unsubscribe
\"
"
;
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
message
+=
",
\"
topic
\"
:
\"
"
+
topic
+
"
\"
"
;
message
=
"{"
+
message
+
"}"
;
std
::
string
client_name
=
"topic_unsubscriber"
+
topic
;
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef ROS_BRIDGE_DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
// unadvertise
connection
->
send_close
(
1000
);
};
start
(
client_name
,
client
,
message
);
service_topic_list
.
erase
(
it_ser_top
);
}
void
RosbridgeWsClient
::
unsubscribeAll
(){
for
(
auto
entry
:
service_topic_list
){
if
(
std
::
get
<
integral
(
EntryEnum
::
EntryType
)
>
(
entry
)
==
EntryType
::
SubscribedTopic
)
{
unsubscribe
(
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
entry
));
}
}
}
void
RosbridgeWsClient
::
advertiseService
(
const
std
::
string
&
client_name
,
const
std
::
string
&
service
,
const
std
::
string
&
type
,
const
InMessage
&
callback
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it_client
=
client_map
.
find
(
client_name
);
if
(
it_client
!=
client_map
.
end
())
{
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
[
service
](
const
EntryData
&
td
){
return
service
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
if
(
it_ser_top
!=
service_topic_list
.
end
()){
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"service: "
<<
service
<<
" already advertised"
<<
std
::
endl
;
#endif
return
;
}
auto
client
=
it_client
->
second
;
std
::
weak_ptr
<
WsClient
>
wpClient
=
client
;
service_topic_list
.
push_back
(
std
::
make_tuple
(
EntryType
::
AdvertisedService
,
service
,
client_name
,
wpClient
));
std
::
string
message
=
"{
\"
op
\"
:
\"
advertise_service
\"
,
\"
service
\"
:
\"
"
+
service
+
"
\"
,
\"
type
\"
:
\"
"
+
type
+
"
\"
}"
;
it_client
->
second
->
on_message
=
callback
;
start
(
client_name
,
it_client
->
second
,
message
);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
}
#endif
}
void
RosbridgeWsClient
::
unadvertiseService
(
const
std
::
string
&
service
){
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
[
service
](
const
EntryData
&
td
){
return
service
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
if
(
it_ser_top
==
service_topic_list
.
end
()){
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"service: "
<<
service
<<
" not advertised"
<<
std
::
endl
;
#endif
return
;
}
std
::
string
message
=
"
\"
op
\"
:
\"
unadvertise_service
\"
"
;
message
+=
",
\"
service
\"
:
\"
"
+
service
+
"
\"
"
;
message
=
"{"
+
message
+
"}"
;
std
::
string
client_name
=
"service_unadvertiser"
+
service
;
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef ROS_BRIDGE_DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
// unadvertise
connection
->
send_close
(
1000
);
};
start
(
client_name
,
client
,
message
);
service_topic_list
.
erase
(
it_ser_top
);
}
void
RosbridgeWsClient
::
unadvertiseAllServices
(){
for
(
auto
entry
:
service_topic_list
){
if
(
std
::
get
<
integral
(
EntryEnum
::
EntryType
)
>
(
entry
)
==
EntryType
::
AdvertisedService
)
{
unadvertiseService
(
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
entry
));
}
}
}
void
RosbridgeWsClient
::
serviceResponse
(
const
std
::
string
&
service
,
const
std
::
string
&
id
,
bool
result
,
const
rapidjson
::
Document
&
values
)
{
std
::
string
message
=
"
\"
op
\"
:
\"
service_response
\"
,
\"
service
\"
:
\"
"
+
service
+
"
\"
,
\"
result
\"
:"
+
((
result
)
?
"true"
:
"false"
);
// Rosbridge somehow does not allow service_response to be sent without id and values
// , so we cannot omit them even though the documentation says they are optional.
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
// Convert JSON document to string
rapidjson
::
StringBuffer
strbuf
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
strbuf
);
values
.
Accept
(
writer
);
message
+=
",
\"
values
\"
:"
+
std
::
string
(
strbuf
.
GetString
());
message
=
"{"
+
message
+
"}"
;
std
::
string
client_name
=
"service_response_client"
+
service
;
std
::
shared_ptr
<
WsClient
>
service_response_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef ROS_BRIDGE_DEBUG
service_response_client
->
on_open
=
[
message
,
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
service_response_client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
connection
->
send_close
(
1000
);
};
start
(
client_name
,
service_response_client
,
message
);
}
void
RosbridgeWsClient
::
callService
(
const
std
::
string
&
service
,
const
InMessage
&
callback
,
const
rapidjson
::
Document
&
args
,
const
std
::
string
&
id
,
int
fragment_size
,
const
std
::
string
&
compression
)
{
std
::
string
message
=
"
\"
op
\"
:
\"
call_service
\"
,
\"
service
\"
:
\"
"
+
service
+
"
\"
"
;
if
(
!
args
.
IsNull
())
{
rapidjson
::
StringBuffer
strbuf
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
strbuf
);
args
.
Accept
(
writer
);
message
+=
",
\"
args
\"
:"
+
std
::
string
(
strbuf
.
GetString
());
}
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
if
(
fragment_size
>
-
1
)
{
message
+=
",
\"
fragment_size
\"
:"
+
std
::
to_string
(
fragment_size
);
}
if
(
compression
.
compare
(
"none"
)
==
0
||
compression
.
compare
(
"png"
)
==
0
)
{
message
+=
",
\"
compression
\"
:
\"
"
+
compression
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
std
::
string
client_name
=
"call_service_client"
+
service
;
std
::
shared_ptr
<
WsClient
>
call_service_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
if
(
callback
)
{
call_service_client
->
on_message
=
callback
;
}
else
{
call_service_client
->
on_message
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
client_name
<<
": Message received: "
<<
in_message
->
string
()
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending close connection"
<<
std
::
endl
;
#else
(
void
)
in_message
;
#endif
connection
->
send_close
(
1000
);
};
}
start
(
client_name
,
call_service_client
,
message
);
}
bool
RosbridgeWsClient
::
serviceAvailable
(
const
std
::
string
&
service
)
{
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
"checking if service "
<<
service
<<
" is available"
<<
std
::
endl
;
#endif
size_t
pos
;
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
pos
=
available_services
.
find
(
service
);
}
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
)
{
waitForService
(
service
,
[]{
return
false
;
// never stop
});
}
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
,
const
std
::
function
<
bool
(
void
)
>
stop
)
{
#ifdef ROS_BRIDGE_DEBUG
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
long
counter
=
0
;
#endif
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
();
while
(
!
stop
()
)
{
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
#ifdef ROS_BRIDGE_DEBUG
++
counter
;
#endif
if
(
serviceAvailable
(
service
)
){
break
;
}
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
else
{
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
}
};
#ifdef ROS_BRIDGE_DEBUG
auto
e
=
std
::
chrono
::
high_resolution_clock
::
now
();
std
::
cout
<<
"waitForService() "
<<
service
<<
" time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
e
-
s
).
count
()
<<
" ms."
<<
std
::
endl
;
std
::
cout
<<
"waitForTopic() "
<<
service
<<
": number of calls to topicAvailable: "
<<
counter
<<
std
::
endl
;
#endif
}
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
)
{
waitForTopic
(
topic
,
[]{
return
false
;
// never stop
});
}
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
function
<
bool
(
void
)
>
stop
)
{
#ifdef ROS_BRIDGE_DEBUG
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
long
counter
=
0
;
#endif
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
();
while
(
!
stop
()
)
{
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
#ifdef ROS_BRIDGE_DEBUG
++
counter
;
#endif
if
(
topicAvailable
(
topic
)
){
break
;
}
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
else
{
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
}
};
#ifdef ROS_BRIDGE_DEBUG
auto
e
=
std
::
chrono
::
high_resolution_clock
::
now
();
std
::
cout
<<
"waitForTopic() "
<<
topic
<<
" time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
e
-
s
).
count
()
<<
" ms."
<<
std
::
endl
;
std
::
cout
<<
"waitForTopic() "
<<
topic
<<
": number of calls to topicAvailable: "
<<
counter
<<
std
::
endl
;
#endif
}
bool
is_valid_port_path
(
std
::
string
server_port_path
)
{
std
::
regex
url_regex
(
"^(((([a-z]|[A-z])+([0-9]|_)*
\\
.*([a-z]|[A-z])+([0-9]|_)*))"
"|(((1?[0-9]{1,2}|2[0-4][0-9]|25[0-5])
\\
.){3}(1?[0-9]{1,2}|2[0-4][0-9]|25[0-5]){1}))"
":[0-9]+$"
);
return
std
::
regex_match
(
server_port_path
,
url_regex
);
}
src/comm/ros_bridge/include/RosBridgeClient.h
View file @
2430ec45
...
...
@@ -11,400 +11,153 @@
#include "rapidjson/include/rapidjson/writer.h"
#include "rapidjson/include/rapidjson/stringbuffer.h"
#include <chrono>
#include <functional>
#include <thread>
#include <future>
#include <mutex>
#include <tuple>
#include <deque>
#include <thread>
bool
is_valid_port_path
(
std
::
string
server_port_path
);
using
WsClient
=
SimpleWeb
::
SocketClient
<
SimpleWeb
::
WS
>
;
using
InMessage
=
std
::
function
<
void
(
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
)
>
;
template
<
typename
T
>
constexpr
typename
std
::
underlying_type
<
T
>::
type
integral
(
T
value
)
{
return
static_cast
<
typename
std
::
underlying_type
<
T
>::
type
>
(
value
);
}
class
RosbridgeWsClient
{
std
::
string
server_port_path
;
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>
client_map
;
std
::
mutex
map_mutex
;
void
start
(
const
std
::
string
&
client_name
,
std
::
shared_ptr
<
WsClient
>
client
,
const
std
::
string
&
message
)
{
if
(
!
client
->
on_open
)
{
#ifdef DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#else
client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#endif
#ifdef DEBUG
std
::
cout
<<
client_name
<<
": Opened connection"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
enum
class
EntryType
{
SubscribedTopic
,
AdvertisedTopic
,
AdvertisedService
,
};
}
#ifdef DEBUG
if
(
!
client
->
on_message
)
{
client
->
on_message
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
std
::
cout
<<
client_name
<<
": Message received: "
<<
in_message
->
string
()
<<
std
::
endl
;
};
}
using
EntryData
=
std
::
tuple
<
EntryType
,
std
::
string
/*service/topic name*/
,
std
::
string
/*client name*/
,
std
::
weak_ptr
<
WsClient
>
/*client*/
>
;
if
(
!
client
->
on_close
)
{
client
->
on_close
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
int
status
,
const
std
::
string
&
/*reason*/
)
{
std
::
cout
<<
client_name
<<
": Closed connection with status code "
<<
status
<<
std
::
endl
;
enum
class
EntryEnum
{
EntryType
=
0
,
ServiceTopicName
=
1
,
ClientName
=
2
,
WPClient
=
3
};
}
if
(
!
client
->
on_error
)
{
// See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings
client
->
on_error
=
[
client_name
](
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
const
SimpleWeb
::
error_code
&
ec
)
{
std
::
cout
<<
client_name
<<
": Error: "
<<
ec
<<
", error message: "
<<
ec
.
message
()
<<
std
::
endl
;
};
}
#endif
#ifdef DEBUG
std
::
thread
client_thread
([
client_name
,
client
]()
{
#else
std
::
thread
client_thread
([
client
]()
{
#endif
client
->
start
();
#ifdef DEBUG
std
::
cout
<<
client_name
<<
": Terminated"
<<
std
::
endl
;
#endif
client
->
on_open
=
NULL
;
client
->
on_message
=
NULL
;
client
->
on_close
=
NULL
;
client
->
on_error
=
NULL
;
});
client_thread
.
detach
();
}
const
std
::
string
server_port_path
;
std
::
unordered_map
<
std
::
string
/*client name*/
,
std
::
shared_ptr
<
WsClient
>
/*client*/
>
client_map
;
std
::
deque
<
EntryData
>
service_topic_list
;
std
::
mutex
mutex
;
std
::
shared_ptr
<
std
::
atomic_bool
>
isConnected
;
std
::
shared_ptr
<
std
::
atomic_bool
>
stopped
;
std
::
string
available_topics
;
std
::
string
available_services
;
std
::
shared_ptr
<
std
::
thread
>
periodic_thread
;
public:
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
)
{
this
->
server_port_path
=
server_port_path
;
}
~
RosbridgeWsClient
()
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
// neccessary?
for
(
auto
&
client
:
client_map
)
{
client
.
second
->
stop
();
client
.
second
.
reset
();
}
}
// The execution can take up to 100 ms!
bool
connected
(){
auto
p
=
std
::
make_shared
<
std
::
promise
<
void
>>
();
auto
future
=
p
->
get_future
();
auto
callback
=
[](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
std
::
promise
<
void
>>
p
)
{
p
->
set_value
();
connection
->
send_close
(
1000
);
};
std
::
shared_ptr
<
WsClient
>
status_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
status_client
->
on_open
=
std
::
bind
(
callback
,
std
::
placeholders
::
_1
,
p
);
std
::
async
([
status_client
]{
status_client
->
start
(
);
status_client
->
on_open
=
NULL
;
status_client
->
on_message
=
NULL
;
status_client
->
on_close
=
NULL
;
status_client
->
on_error
=
NULL
;
void
start
(
const
std
::
string
&
client_name
,
std
::
shared_ptr
<
WsClient
>
client
,
const
std
::
string
&
message
);
public:
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
)
;
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
,
bool
run
)
;
}
);
~
RosbridgeWsClient
(
);
auto
status
=
future
.
wait_for
(
std
::
chrono
::
milliseconds
(
100
));
return
status
==
std
::
future_status
::
ready
;
}
bool
connected
();
void
run
();
void
stop
();
void
reset
();
// Adds a client to the client_map.
void
addClient
(
const
std
::
string
&
client_name
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
==
client_map
.
end
())
{
client_map
[
client_name
]
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
}
#ifdef DEBUG
else
{
std
::
cerr
<<
client_name
<<
" has already been created"
<<
std
::
endl
;
}
#endif
}
std
::
shared_ptr
<
WsClient
>
getClient
(
const
std
::
string
&
client_name
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
return
it
->
second
;
}
return
NULL
;
}
void
stopClient
(
const
std
::
string
&
client_name
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
it
->
second
->
stop
();
it
->
second
->
on_open
=
NULL
;
it
->
second
->
on_message
=
NULL
;
it
->
second
->
on_close
=
NULL
;
it
->
second
->
on_error
=
NULL
;
#ifdef DEBUG
std
::
cout
<<
client_name
<<
" has been stopped"
<<
std
::
endl
;
#endif
}
#ifdef DEBUG
else
{
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
}
#endif
}
void
removeClient
(
const
std
::
string
&
client_name
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving the client have been launched.
std
::
thread
t
([](
std
::
shared_ptr
<
WsClient
>
client
){
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
100
));
client
->
stop
();
client
.
reset
();
},
it
->
second
/*lambda param*/
);
client_map
.
erase
(
it
);
t
.
detach
();
#ifdef DEBUG
std
::
cout
<<
client_name
<<
" has been removed"
<<
std
::
endl
;
#endif
}
#ifdef DEBUG
else
{
std
::
cerr
<<
client_name
<<
" has not been created"
<<
std
::
endl
;
}
#endif
}
void
addClient
(
const
std
::
string
&
client_name
);
std
::
shared_ptr
<
WsClient
>
getClient
(
const
std
::
string
&
client_name
);
void
stopClient
(
const
std
::
string
&
client_name
);
void
removeClient
(
const
std
::
string
&
client_name
);
//!
//! \brief Returns a string containing all advertised topics.
//! \return Returns a string containing all advertised topics.
//!
//! \note This function will wait until the /rosapi/topics service is available.
//! \note Call connected() in advance to ensure that a connection was established.
//! \pre Connection must be established, \see \fn connected().
//!
std
::
string
getAdvertisedTopics
();
//!
//! \brief Returns a string containing all advertised services.
//! \return Returns a string containing all advertised services.
//!
//! \note This function will wait until the /rosapi/services service is available.
//! \note Call connected() in advance to ensure that a connection was established.
//!
std
::
string
getAdvertisedServices
();
bool
topicAvailable
(
const
std
::
string
&
topic
);
// Gets the client from client_map and starts it. Advertising is essentially sending a message.
// One client per topic!
void
advertise
(
const
std
::
string
&
client_name
,
const
std
::
string
&
topic
,
const
std
::
string
&
type
,
const
std
::
string
&
id
=
""
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
std
::
string
message
=
"
\"
op
\"
:
\"
advertise
\"
,
\"
topic
\"
:
\"
"
+
topic
+
"
\"
,
\"
type
\"
:
\"
"
+
type
+
"
\"
"
;
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
start
(
client_name
,
it
->
second
,
message
);
}
#ifdef DEBUG
else
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
}
#endif
}
void
publish
(
const
std
::
string
&
topic
,
const
rapidjson
::
Document
&
msg
,
const
std
::
string
&
id
=
""
)
{
rapidjson
::
StringBuffer
strbuf
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
strbuf
);
msg
.
Accept
(
writer
);
std
::
string
message
=
"
\"
op
\"
:
\"
publish
\"
,
\"
topic
\"
:
\"
"
+
topic
+
"
\"
,
\"
msg
\"
:"
+
strbuf
.
GetString
();
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
std
::
shared_ptr
<
WsClient
>
publish_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
publish_client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#ifdef DEBUG
std
::
cout
<<
"publish_client: Opened connection"
<<
std
::
endl
;
std
::
cout
<<
"publish_client: Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
// TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
connection
->
send_close
(
1000
);
};
void
advertise
(
const
std
::
string
&
client_name
,
const
std
::
string
&
topic
,
const
std
::
string
&
type
,
const
std
::
string
&
id
=
""
);
start
(
"publish_client"
,
publish_client
,
message
);
}
void
subscribe
(
const
std
::
string
&
client_name
,
const
std
::
string
&
topic
,
const
InMessage
&
callback
,
const
std
::
string
&
id
=
""
,
const
std
::
string
&
type
=
""
,
int
throttle_rate
=
-
1
,
int
queue_length
=
-
1
,
int
fragment_size
=
-
1
,
const
std
::
string
&
compression
=
""
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
std
::
string
message
=
"
\"
op
\"
:
\"
subscribe
\"
,
\"
topic
\"
:
\"
"
+
topic
+
"
\"
"
;
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
if
(
type
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
type
\"
:
\"
"
+
type
+
"
\"
"
;
}
if
(
throttle_rate
>
-
1
)
{
message
+=
",
\"
throttle_rate
\"
:"
+
std
::
to_string
(
throttle_rate
);
}
if
(
queue_length
>
-
1
)
{
message
+=
",
\"
queue_length
\"
:"
+
std
::
to_string
(
queue_length
);
}
if
(
fragment_size
>
-
1
)
{
message
+=
",
\"
fragment_size
\"
:"
+
std
::
to_string
(
fragment_size
);
}
if
(
compression
.
compare
(
"none"
)
==
0
||
compression
.
compare
(
"png"
)
==
0
)
{
message
+=
",
\"
compression
\"
:
\"
"
+
compression
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
it
->
second
->
on_message
=
callback
;
start
(
client_name
,
it
->
second
,
message
);
}
#ifdef DEBUG
else
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
}
#endif
}
void
advertiseService
(
const
std
::
string
&
client_name
,
const
std
::
string
&
service
,
const
std
::
string
&
type
,
const
InMessage
&
callback
)
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
map_mutex
);
std
::
unordered_map
<
std
::
string
,
std
::
shared_ptr
<
WsClient
>>::
iterator
it
=
client_map
.
find
(
client_name
);
if
(
it
!=
client_map
.
end
())
{
std
::
string
message
=
"{
\"
op
\"
:
\"
advertise_service
\"
,
\"
service
\"
:
\"
"
+
service
+
"
\"
,
\"
type
\"
:
\"
"
+
type
+
"
\"
}"
;
it
->
second
->
on_message
=
callback
;
start
(
client_name
,
it
->
second
,
message
);
}
#ifdef DEBUG
else
{
std
::
cerr
<<
client_name
<<
"has not been created"
<<
std
::
endl
;
}
#endif
}
void
serviceResponse
(
const
std
::
string
&
service
,
const
std
::
string
&
id
,
bool
result
,
const
rapidjson
::
Document
&
values
=
{})
{
std
::
string
message
=
"
\"
op
\"
:
\"
service_response
\"
,
\"
service
\"
:
\"
"
+
service
+
"
\"
,
\"
result
\"
:"
+
((
result
)
?
"true"
:
"false"
);
// Rosbridge somehow does not allow service_response to be sent without id and values
// , so we cannot omit them even though the documentation says they are optional.
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
// Convert JSON document to string
rapidjson
::
StringBuffer
strbuf
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
strbuf
);
values
.
Accept
(
writer
);
message
+=
",
\"
values
\"
:"
+
std
::
string
(
strbuf
.
GetString
());
message
=
"{"
+
message
+
"}"
;
std
::
shared_ptr
<
WsClient
>
service_response_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
service_response_client
->
on_open
=
[
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
#ifdef DEBUG
std
::
cout
<<
"service_response_client: Opened connection"
<<
std
::
endl
;
std
::
cout
<<
"service_response_client: Sending message: "
<<
message
<<
std
::
endl
;
#endif
connection
->
send
(
message
);
connection
->
send_close
(
1000
);
};
//!
//! \brief Unadvertises the topice \p topic
//! \param topic The topic to be unadvertised
//! \param id
//! \pre The topic \p topic must be advertised, \see topicAvailable().
//!
void
unadvertise
(
const
std
::
string
&
topic
,
const
std
::
string
&
id
=
""
);
start
(
"service_response_client"
,
service_response_client
,
message
);
}
void
callService
(
const
std
::
string
&
service
,
const
InMessage
&
callback
,
const
rapidjson
::
Document
&
args
=
{},
const
std
::
string
&
id
=
""
,
int
fragment_size
=
-
1
,
const
std
::
string
&
compression
=
""
)
{
std
::
string
message
=
"
\"
op
\"
:
\"
call_service
\"
,
\"
service
\"
:
\"
"
+
service
+
"
\"
"
;
if
(
!
args
.
IsNull
())
{
rapidjson
::
StringBuffer
strbuf
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
strbuf
);
args
.
Accept
(
writer
);
message
+=
",
\"
args
\"
:"
+
std
::
string
(
strbuf
.
GetString
());
}
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
if
(
fragment_size
>
-
1
)
{
message
+=
",
\"
fragment_size
\"
:"
+
std
::
to_string
(
fragment_size
);
}
if
(
compression
.
compare
(
"none"
)
==
0
||
compression
.
compare
(
"png"
)
==
0
)
{
message
+=
",
\"
compression
\"
:
\"
"
+
compression
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
std
::
shared_ptr
<
WsClient
>
call_service_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
if
(
callback
)
{
call_service_client
->
on_message
=
callback
;
}
else
{
call_service_client
->
on_message
=
[](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
#ifdef DEBUG
std
::
cout
<<
"call_service_client: Message received: "
<<
in_message
->
string
()
<<
std
::
endl
;
std
::
cout
<<
"call_service_client: Sending close connection"
<<
std
::
endl
;
#endif
connection
->
send_close
(
1000
);
};
}
void
unadvertiseAll
();
//!
//! \brief Publishes the message \p msg to the topic \p topic.
//! \param topic The topic to publish the message.
//! \param msg The message to publish.
//! \param id
//! \pre The topic \p topic must be advertised, \see topicAvailable().
//!
void
publish
(
const
std
::
string
&
topic
,
const
rapidjson
::
Document
&
msg
,
const
std
::
string
&
id
=
""
);
//!
//! \brief Subscribes the client \p client_name to the topic \p topic.
//! \param client_name
//! \param topic
//! \param callback
//! \param id
//! \param type
//! \param throttle_rate
//! \param queue_length
//! \param fragment_size
//! \param compression
//! \pre The topic \p topic must be advertised, \see topicAvailable().
//!
void
subscribe
(
const
std
::
string
&
client_name
,
const
std
::
string
&
topic
,
const
InMessage
&
callback
,
const
std
::
string
&
id
=
""
,
const
std
::
string
&
type
=
""
,
int
throttle_rate
=
-
1
,
int
queue_length
=
-
1
,
int
fragment_size
=
-
1
,
const
std
::
string
&
compression
=
""
);
void
unsubscribe
(
const
std
::
string
&
topic
,
const
std
::
string
&
id
=
""
);
void
unsubscribeAll
();
void
advertiseService
(
const
std
::
string
&
client_name
,
const
std
::
string
&
service
,
const
std
::
string
&
type
,
const
InMessage
&
callback
);
void
unadvertiseService
(
const
std
::
string
&
service
);
void
unadvertiseAllServices
();
start
(
"call_service_client"
,
call_service_client
,
message
);
}
void
serviceResponse
(
const
std
::
string
&
service
,
const
std
::
string
&
id
,
bool
result
,
const
rapidjson
::
Document
&
values
);
void
callService
(
const
std
::
string
&
service
,
const
InMessage
&
callback
,
const
rapidjson
::
Document
&
args
=
{},
const
std
::
string
&
id
=
""
,
int
fragment_size
=
-
1
,
const
std
::
string
&
compression
=
""
);
//!
//! \brief Checks if the service \p service is available.
...
...
@@ -412,104 +165,34 @@ public:
//! \return Returns true if the service is available, false either.
//! \note Don't call this function to frequently. Use \fn waitForService() instead.
//!
bool
serviceAvailable
(
const
std
::
string
&
service
)
{
const
rapidjson
::
Document
args
=
{};
const
std
::
string
&
id
=
""
;
const
int
fragment_size
=
-
1
;
const
std
::
string
&
compression
=
""
;
std
::
string
message
=
"
\"
op
\"
:
\"
call_service
\"
,
\"
service
\"
:
\"
"
+
service
+
"
\"
"
;
std
::
string
client_name
(
"wait_for_service_client"
);
if
(
!
args
.
IsNull
())
{
rapidjson
::
StringBuffer
strbuf
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
strbuf
);
args
.
Accept
(
writer
);
message
+=
",
\"
args
\"
:"
+
std
::
string
(
strbuf
.
GetString
());
}
if
(
id
.
compare
(
""
)
!=
0
)
{
message
+=
",
\"
id
\"
:
\"
"
+
id
+
"
\"
"
;
}
if
(
fragment_size
>
-
1
)
{
message
+=
",
\"
fragment_size
\"
:"
+
std
::
to_string
(
fragment_size
);
}
if
(
compression
.
compare
(
"none"
)
==
0
||
compression
.
compare
(
"png"
)
==
0
)
{
message
+=
",
\"
compression
\"
:
\"
"
+
compression
+
"
\"
"
;
}
message
=
"{"
+
message
+
"}"
;
std
::
shared_ptr
<
WsClient
>
wait_for_service_client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
std
::
shared_ptr
<
std
::
promise
<
bool
>>
p
(
std
::
make_shared
<
std
::
promise
<
bool
>>
());
auto
future
=
p
->
get_future
();
#ifdef DEBUG
wait_for_service_client
->
on_message
=
[
p
,
service
,
client_name
](
#else
wait_for_service_client
->
on_message
=
[
p
](
#endif
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
#ifdef DEBUG
#endif
rapidjson
::
Document
doc
;
doc
.
Parse
(
in_message
->
string
().
c_str
());
if
(
!
doc
.
HasParseError
()
&&
doc
.
HasMember
(
"result"
)
&&
doc
[
"result"
].
IsBool
()
&&
doc
[
"result"
]
==
true
)
{
#ifdef DEBUG
std
::
cout
<<
client_name
<<
": "
<<
"service "
<<
service
<<
" available."
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
": "
<<
"message: "
<<
in_message
->
string
()
<<
std
::
endl
;
#endif
p
->
set_value
(
true
);
}
else
{
p
->
set_value
(
false
);
}
connection
->
send_close
(
1000
);
};
start
(
client_name
,
wait_for_service_client
,
message
);
return
future
.
get
();
}
bool
serviceAvailable
(
const
std
::
string
&
service
);
//!
//! \brief Waits until the service with the name \p service is available.
//! \param service Service name.
//! @note This function will block as long as the service is not available.
//!
void
waitForService
(
const
std
::
string
&
service
)
{
#ifdef DEBUG
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
long
counter
=
0
;
#endif
while
(
true
)
{
#ifdef DEBUG
++
counter
;
#endif
if
(
serviceAvailable
(
service
)){
break
;
}
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
50
));
// Avoid excessive polling.
};
#ifdef DEBUG
auto
e
=
std
::
chrono
::
high_resolution_clock
::
now
();
std
::
cout
<<
"waitForService(): time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
e
-
s
).
count
()
<<
" ms."
<<
std
::
endl
;
std
::
cout
<<
"waitForService(): clients launched: "
<<
counter
<<
std
::
endl
;
#endif
}
void
waitForService
(
const
std
::
string
&
service
);
//!
//! \brief Waits until the service with the name \p service is available.
//! \param service Service name.
//! \param stop Flag to stop waiting.
//! @note This function will block as long as the service is not available or \p stop is false.
//!
void
waitForService
(
const
std
::
string
&
service
,
const
std
::
function
<
bool
(
void
)
>
stop
);
//!
//! \brief Waits until the topic with the name \p topic is available.
//! \param topic Topic name.
//! @note This function will block as long as the topic is not available.
//!
void
waitForTopic
(
const
std
::
string
&
topic
);
//!
//! \brief Waits until the topic with the name \p topic is available.
//! \param topic Topic name.
//! \param stop Flag to stop waiting.
//! @note This function will block as long as the topic is not available or \p stop is false.
//!
void
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
function
<
bool
(
void
)
>
stop
);
};
src/comm/ros_bridge/include/ThreadSafeQueue.h
deleted
100644 → 0
View file @
0e34b591
#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 @
0e34b591
#include "TopicPublisher.h"
struct
ROSBridge
::
ComPrivate
::
ThreadData
{
const
ROSBridge
::
CasePacker
&
casePacker
;
RosbridgeWsClient
&
rbc
;
ROSBridge
::
ComPrivate
::
JsonQueue
&
queue
;
std
::
mutex
&
queueMutex
;
const
std
::
atomic
<
bool
>
&
running
;
std
::
condition_variable
&
cv
;
};
ROSBridge
::
ComPrivate
::
TopicPublisher
::
TopicPublisher
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
)
:
_running
(
false
)
,
_casePacker
(
casePacker
)
,
_rbc
(
rbc
)
{
}
ROSBridge
::
ComPrivate
::
TopicPublisher
::~
TopicPublisher
()
{
this
->
reset
();
}
void
ROSBridge
::
ComPrivate
::
TopicPublisher
::
start
()
{
if
(
_running
.
load
()
)
// start called while thread running.
return
;
_running
.
store
(
true
);
ROSBridge
::
ComPrivate
::
ThreadData
data
{
_casePacker
,
_rbc
,
_queue
,
_queueMutex
,
_running
,
_cv
};
_pThread
=
std
::
make_unique
<
std
::
thread
>
(
&
ROSBridge
::
ComPrivate
::
transmittLoop
,
data
);
}
void
ROSBridge
::
ComPrivate
::
TopicPublisher
::
reset
()
{
if
(
!
_running
.
load
()
)
// stop called while thread not running.
return
;
_running
.
store
(
false
);
_cv
.
notify_one
();
// Wake publisher thread.
if
(
!
_pThread
)
return
;
_pThread
->
join
();
_queue
.
clear
();
}
void
ROSBridge
::
ComPrivate
::
transmittLoop
(
ROSBridge
::
ComPrivate
::
ThreadData
data
)
{
// Init.
ClientMap
clientMap
;
// Main Loop.
while
(
data
.
running
.
load
()){
std
::
unique_lock
<
std
::
mutex
>
lk
(
data
.
queueMutex
);
// Check if new data available, wait if not.
if
(
data
.
queue
.
empty
()){
data
.
cv
.
wait
(
lk
);
// Wait for condition, spurious wakeups don't matter in this case.
continue
;
}
// Pop message from queue.
JsonDocUPtr
pJsonDoc
(
std
::
move
(
data
.
queue
.
front
()));
data
.
queue
.
pop_front
();
lk
.
unlock
();
// Get tag from Json message and remove it.
Tag
tag
;
bool
ret
=
data
.
casePacker
.
getTag
(
pJsonDoc
,
tag
);
assert
(
ret
);
// Json message does not contain a tag;
(
void
)
ret
;
data
.
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
();
HashType
hash
=
ROSBridge
::
ComPrivate
::
getHash
(
clientName
);
auto
it
=
clientMap
.
find
(
hash
);
if
(
it
==
clientMap
.
end
())
{
// Need to advertise topic?
clientMap
.
insert
(
std
::
make_pair
(
hash
,
clientName
));
std
::
cout
<<
clientName
<<
";"
<<
tag
.
topic
()
<<
";"
<<
tag
.
messageType
()
<<
";"
<<
std
::
endl
;
data
.
rbc
.
addClient
(
clientName
);
data
.
rbc
.
advertise
(
clientName
,
tag
.
topic
(),
tag
.
messageType
()
);
}
// Publish Json message.
data
.
rbc
.
publish
(
tag
.
topic
(),
*
pJsonDoc
.
get
());
}
// while loop
// Tidy up.
for
(
auto
&
it
:
clientMap
)
data
.
rbc
.
removeClient
(
it
.
second
);
}
src/comm/ros_bridge/include/TopicSubscriber.cpp
deleted
100644 → 0
View file @
0e34b591
#include "TopicSubscriber.h"
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
TopicSubscriber
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
)
:
_casePacker
(
casePacker
)
,
_rbc
(
rbc
)
,
_running
(
false
)
{
}
ROSBridge
::
ComPrivate
::
TopicSubscriber
::~
TopicSubscriber
()
{
this
->
reset
();
}
void
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
start
()
{
_running
=
true
;
}
void
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
reset
()
{
if
(
_running
){
_running
=
false
;
{
for
(
std
::
string
clientName
:
_clientList
){
_rbc
.
removeClient
(
clientName
);
}
}
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
_callbackMapStruct
.
mutex
);
_callbackMapStruct
.
map
.
clear
();
}
_clientList
.
clear
();
}
}
bool
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
{
if
(
!
_running
)
return
false
;
std
::
string
clientName
=
ROSBridge
::
ComPrivate
::
_topicSubscriberKey
+
std
::
string
(
topic
);
_clientList
.
push_back
(
clientName
);
HashType
hash
=
getHash
(
clientName
);
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
_callbackMapStruct
.
mutex
);
auto
ret
=
_callbackMapStruct
.
map
.
insert
(
std
::
make_pair
(
hash
,
callback
));
//
if
(
!
ret
.
second
)
return
false
;
// Topic subscription already present.
}
using
namespace
std
::
placeholders
;
auto
f
=
std
::
bind
(
&
ROSBridge
::
ComPrivate
::
subscriberCallback
,
hash
,
std
::
ref
(
_callbackMapStruct
),
_1
,
_2
);
// std::cout << std::endl;
// std::cout << "topic subscription" << std::endl;
// std::cout << "client name: " << clientName << std::endl;
// std::cout << "topic: " << topic << std::endl;
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_rbc
.
addClient
(
clientName
);
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
std
::
cout
<<
"add client time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
<<
" ms"
<<
std
::
endl
;
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_rbc
.
subscribe
(
clientName
,
topic
,
f
);
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
std
::
cout
<<
"subscribe time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
<<
" ms"
<<
std
::
endl
;
}
return
true
;
}
using
WsClient
=
SimpleWeb
::
SocketClient
<
SimpleWeb
::
WS
>
;
void
ROSBridge
::
ComPrivate
::
subscriberCallback
(
const
HashType
&
hash
,
ROSBridge
::
ComPrivate
::
MapStruct
&
mapWrapper
,
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
// Parse document.
JsonDoc
docFull
;
docFull
.
Parse
(
in_message
->
string
().
c_str
());
if
(
docFull
.
HasParseError
()
)
{
std
::
cout
<<
"Json document has parse error: "
<<
in_message
->
string
()
<<
std
::
endl
;
return
;
}
else
if
(
!
docFull
.
HasMember
(
"msg"
))
{
std
::
cout
<<
"Json document does not contain a message (
\"
msg
\"
): "
<<
in_message
->
string
()
<<
std
::
endl
;
return
;
}
// std::cout << "Json document: "
// << in_message->string()
// << std::endl;
// Search callback.
CallbackType
callback
;
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
mapWrapper
.
mutex
);
auto
it
=
mapWrapper
.
map
.
find
(
hash
);
if
(
it
==
mapWrapper
.
map
.
end
())
{
//assert(false); // callback not found
return
;
}
callback
=
it
->
second
;
}
// Extract message and call callback.
JsonDocUPtr
pDoc
(
new
JsonDoc
());
pDoc
->
CopyFrom
(
docFull
[
"msg"
].
Move
(),
docFull
.
GetAllocator
());
callback
(
std
::
move
(
pDoc
));
return
;
}
src/comm/ros_bridge/include/TypeFactory.h
deleted
100644 → 0
View file @
0e34b591
#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 @
2430ec45
#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 @
2430ec45
#pragma once
#include "ros_bridge/include/MessageTag.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <memory>
#include <deque>
#include <unordered_map>
namespace
ROSB
ridge
{
namespace
ComP
rivate
{
namespace
ros_b
ridge
{
namespace
com_p
rivate
{
typedef
MessageTag
Tag
;
typedef
rapidjson
::
Document
JsonDoc
;
typedef
std
::
unique_ptr
<
JsonDoc
>
JsonDocUPtr
;
typedef
std
::
deque
<
JsonDocUPtr
>
JsonQueue
;
...
...
@@ -20,11 +18,17 @@ using ClientMap = std::unordered_map<HashType, std::string>;
static
const
char
*
_topicAdvertiserKey
=
"topic_advertiser"
;
static
const
char
*
_topicPublisherKey
=
"topic_publisher"
;
//static const char* _topic
AdvertiserKey = "service_advertiser";
static
const
char
*
_service
AdvertiserKey
=
"service_advertiser"
;
static
const
char
*
_topicSubscriberKey
=
"topic_subscriber"
;
std
::
size_t
getHash
(
const
std
::
string
&
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 @
2430ec45
#pragma once
namespace
ROSB
ridge
{
namespace
ros_b
ridge
{
namespace
traits
{
...
...
@@ -182,5 +182,19 @@ struct Select<0, T, U> {typedef U Result;};
struct
HasComponents
{};
struct
Has3Components
:
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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#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 @
2430ec45
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/CasePacker.h"
#include "ros_bridge/include/TypeFactory.h"
#include "ros_bridge/include/JsonFactory.h"
#include "ros_bridge/include/TopicPublisher.h"
#include "ros_bridge/include/TopicSubscriber.h"
#include "ros_bridge/include/topic_publisher.h"
#include "ros_bridge/include/topic_subscriber.h"
#include "ros_bridge/include/server.h"
#include <memory>
#include <
tuple
>
#include <
functional
>
#include "boost/lockfree/queue.hpp"
namespace
ROSBridge
{
namespace
ros_bridge
{
class
ROSBridge
{
public:
typedef
MessageTag
Tag
;
typedef
rapidjson
::
Document
JsonDoc
;
typedef
std
::
unique_ptr
<
JsonDoc
>
JsonDocUPtr
;
explicit
ROSBridge
();
template
<
class
T
>
void
publish
(
T
&
msg
,
const
std
::
string
&
topic
){
_topicPublisher
.
publish
(
msg
,
topic
);
}
void
publish
(
JsonDocUPtr
doc
);
void
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
);
const
CasePacker
*
casePacker
()
const
;
explicit
ROSBridge
(
const
char
*
connectionString
);
void
publish
(
JsonDocUPtr
doc
,
const
char
*
topic
);
void
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
);
void
advertiseService
(
const
char
*
service
,
const
char
*
type
,
const
std
::
function
<
JsonDocUPtr
(
JsonDocUPtr
)
>
&
callback
);
void
advertiseTopic
(
const
char
*
topic
,
const
char
*
type
);
//! @brief Start ROS bridge.
void
start
();
//! @brief Stops ROS bridge.
void
reset
();
//! @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
isRunning
();
private:
TypeFactory
_typeFactory
;
JsonFactory
_jsonFactory
;
CasePacker
_casePacker
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
RosbridgeWsClient
_rbc
;
ComPrivate
::
TopicPublisher
_topicPublisher
;
ComPrivate
::
TopicSubscriber
_topicSubscriber
;
com_private
::
TopicPublisher
_topicPublisher
;
com_private
::
TopicSubscriber
_topicSubscriber
;
com_private
::
Server
_server
;
};
bool
isValidConnectionString
(
const
char
*
connectionString
);
}
src/comm/ros_bridge/include/server.cpp
0 → 100644
View file @
2430ec45
#include "server.h"
#include "rapidjson/include/rapidjson/ostreamwrapper.h"
ros_bridge
::
com_private
::
Server
::
Server
(
RosbridgeWsClient
&
rbc
)
:
_rbc
(
rbc
)
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
}
void
ros_bridge
::
com_private
::
Server
::
advertiseService
(
const
std
::
string
&
service
,
const
std
::
string
&
type
,
const
Server
::
CallbackType
&
userCallback
)
{
std
::
string
clientName
=
_serviceAdvertiserKey
+
service
;
auto
it
=
_serviceMap
.
find
(
clientName
);
if
(
it
!=
_serviceMap
.
end
())
return
;
// Service allready advertised.
_serviceMap
.
insert
(
std
::
make_pair
(
clientName
,
service
));
_rbc
.
addClient
(
clientName
);
auto
rbc
=
&
_rbc
;
_rbc
.
advertiseService
(
clientName
,
service
,
type
,
[
userCallback
,
rbc
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
// message->string() is destructive, so we have to buffer it first
std
::
string
messagebuf
=
in_message
->
string
();
//std::cout << "advertiseServiceCallback(): Message Received: "
// << messagebuf << std::endl;
rapidjson
::
Document
document
;
if
(
document
.
Parse
(
messagebuf
.
c_str
()).
HasParseError
())
{
std
::
cerr
<<
"advertiseServiceCallback(): Error in parsing service request message: "
<<
messagebuf
<<
std
::
endl
;
return
;
}
if
(
!
document
.
HasMember
(
"args"
)
||
!
document
[
"args"
].
IsObject
()){
std
::
cerr
<<
"advertiseServiceCallback(): message has no args member: "
<<
messagebuf
<<
std
::
endl
;
return
;
}
if
(
!
document
.
HasMember
(
"service"
)
||
!
document
[
"service"
].
IsString
()){
std
::
cerr
<<
"advertiseServiceCallback(): message has no service member: "
<<
messagebuf
<<
std
::
endl
;
return
;
}
if
(
!
document
.
HasMember
(
"id"
)
||
!
document
[
"id"
].
IsString
()){
std
::
cerr
<<
"advertiseServiceCallback(): message has no id member: "
<<
messagebuf
<<
std
::
endl
;
return
;
}
JsonDocUPtr
pDoc
(
new
JsonDoc
());
std
::
cout
<<
"args member count: "
<<
document
[
"args"
].
MemberCount
();
if
(
document
[
"args"
].
MemberCount
()
>
0
)
pDoc
->
CopyFrom
(
document
[
"args"
].
Move
(),
document
.
GetAllocator
());
JsonDocUPtr
pDocResponse
=
userCallback
(
std
::
move
(
pDoc
));
rbc
->
serviceResponse
(
document
[
"service"
].
GetString
(),
document
[
"id"
].
GetString
(),
pDocResponse
->
MemberCount
()
>
0
?
true
:
false
,
*
pDocResponse
);
return
;
});
}
ros_bridge
::
com_private
::
Server
::~
Server
()
{
this
->
reset
();
}
void
ros_bridge
::
com_private
::
Server
::
start
()
{
_stopped
->
store
(
false
);
}
void
ros_bridge
::
com_private
::
Server
::
reset
()
{
if
(
_stopped
->
load
()
)
return
;
std
::
cout
<<
"Server: _stopped->store(true)"
<<
std
::
endl
;
_stopped
->
store
(
true
);
for
(
const
auto
&
item
:
_serviceMap
){
std
::
cout
<<
"Server: unadvertiseService "
<<
item
.
second
<<
std
::
endl
;
_rbc
.
unadvertiseService
(
item
.
second
);
std
::
cout
<<
"Server: removeClient "
<<
item
.
first
<<
std
::
endl
;
_rbc
.
removeClient
(
item
.
first
);
}
std
::
cout
<<
"Server: _serviceMap cleared "
<<
std
::
endl
;
_serviceMap
.
clear
();
}
src/comm/ros_bridge/include/server.h
0 → 100644
View file @
2430ec45
#pragma once
#include "ros_bridge/include/com_private.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include <unordered_map>
namespace
ros_bridge
{
namespace
com_private
{
class
Server
{
typedef
std
::
unordered_map
<
std
::
string
,
std
::
string
>
ServiceMap
;
public:
typedef
std
::
function
<
JsonDocUPtr
(
JsonDocUPtr
)
>
CallbackType
;
Server
()
=
delete
;
Server
(
RosbridgeWsClient
&
rbc
);
~
Server
();
//! @brief Starts the subscriber.
void
start
();
//! @brief Resets the subscriber.
void
reset
();
void
advertiseService
(
const
std
::
string
&
service
,
const
std
::
string
&
type
,
const
CallbackType
&
userCallback
);
private:
RosbridgeWsClient
&
_rbc
;
ServiceMap
_serviceMap
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
};
}
// namespace ComPrivate
}
// namespace ROSBridge
src/comm/ros_bridge/include/topic_publisher.cpp
0 → 100644
View file @
2430ec45
#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 @
2430ec45
#pragma once
#include "ros_bridge/include/TypeFactory.h"
#include "ros_bridge/include/CasePacker.h"
#include "ros_bridge/include/ComPrivateInclude.h"
#include "ros_bridge/include/com_private.h"
#include "ros_bridge/include/RosBridgeClient.h"
#include <thread>
#include <deque>
#include <atomic>
#include <mutex>
#include <set>
#include <condition_variable>
namespace
ROSB
ridge
{
namespace
ComP
rivate
{
namespace
ros_b
ridge
{
namespace
com_p
rivate
{
struct
ThreadData
;
...
...
@@ -24,8 +20,7 @@ class TopicPublisher
public:
TopicPublisher
()
=
delete
;
TopicPublisher
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
);
TopicPublisher
(
RosbridgeWsClient
&
rbc
);
~
TopicPublisher
();
...
...
@@ -35,33 +30,20 @@ public:
//! @brief Resets the publisher.
void
reset
();
void
publish
(
JsonDocUPtr
docPtr
){
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_queueMutex
);
_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
));
}
void
publish
(
JsonDocUPtr
docPtr
,
const
char
*
topic
);
bool
advertise
(
const
char
*
topic
,
const
char
*
type
);
private:
using
TopicMap
=
std
::
unordered_map
<
std
::
string
,
std
::
string
>
;
JsonQueue
_queue
;
std
::
mutex
_queueMutex
;
std
::
atomic
<
bool
>
_running
;
CasePacker
&
_casePacker
;
TopicMap
_topicMap
;
std
::
mutex
_mutex
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
RosbridgeWsClient
&
_rbc
;
CondVar
_cv
;
ThreadPtr
_pThread
;
};
void
transmittLoop
(
ThreadData
data
);
}
// namespace CommunicatorDetail
}
// namespace ROSBridge
src/comm/ros_bridge/include/topic_subscriber.cpp
0 → 100644
View file @
2430ec45
#include "topic_subscriber.h"
#include <thread>
ros_bridge
::
com_private
::
TopicSubscriber
::
TopicSubscriber
(
RosbridgeWsClient
&
rbc
)
:
_rbc
(
rbc
)
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
}
ros_bridge
::
com_private
::
TopicSubscriber
::~
TopicSubscriber
()
{
this
->
reset
();
}
void
ros_bridge
::
com_private
::
TopicSubscriber
::
start
()
{
_stopped
->
store
(
false
);
}
void
ros_bridge
::
com_private
::
TopicSubscriber
::
reset
()
{
if
(
!
_stopped
->
load
()
){
_stopped
->
store
(
true
);
{
for
(
auto
&
item
:
_topicMap
){
_rbc
.
unsubscribe
(
item
.
second
);
_rbc
.
removeClient
(
item
.
first
);
}
}
_topicMap
.
clear
();
}
}
void
ros_bridge
::
com_private
::
TopicSubscriber
::
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
{
if
(
_stopped
->
load
()
)
return
;
std
::
string
clientName
=
ros_bridge
::
com_private
::
_topicSubscriberKey
+
std
::
string
(
topic
);
auto
it
=
_topicMap
.
find
(
clientName
);
if
(
it
!=
_topicMap
.
end
()
){
// Topic already subscribed?
return
;
}
_topicMap
.
insert
(
std
::
make_pair
(
clientName
,
std
::
string
(
topic
)));
// Wrap callback.
auto
callbackWrapper
=
[
callback
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
// Parse document.
JsonDoc
docFull
;
docFull
.
Parse
(
in_message
->
string
().
c_str
());
if
(
docFull
.
HasParseError
()
)
{
std
::
cout
<<
"Json document has parse error: "
<<
in_message
->
string
()
<<
std
::
endl
;
return
;
}
else
if
(
!
docFull
.
HasMember
(
"msg"
))
{
std
::
cout
<<
"Json document does not contain a message (
\"
msg
\"
): "
<<
in_message
->
string
()
<<
std
::
endl
;
return
;
}
// Extract message and call callback.
JsonDocUPtr
pDoc
(
new
JsonDoc
());
pDoc
->
CopyFrom
(
docFull
[
"msg"
].
Move
(),
docFull
.
GetAllocator
());
callback
(
std
::
move
(
pDoc
));
};
if
(
!
_rbc
.
topicAvailable
(
topic
)
){
// Wait until topic is available.
std
::
cout
<<
"TopicSubscriber: Starting wait thread, "
<<
clientName
<<
std
::
endl
;
std
::
thread
t
([
this
,
clientName
,
topic
,
callbackWrapper
]{
this
->
_rbc
.
waitForTopic
(
topic
,
[
this
]{
return
this
->
_stopped
->
load
();
});
if
(
!
this
->
_stopped
->
load
()
){
this
->
_rbc
.
addClient
(
clientName
);
this
->
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
std
::
cout
<<
"TopicSubscriber: wait thread subscription successfull: "
<<
clientName
<<
std
::
endl
;
}
std
::
cout
<<
"TopicSubscriber: wait thread end, "
<<
clientName
<<
std
::
endl
;
});
t
.
detach
();
}
else
{
_rbc
.
addClient
(
clientName
);
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
std
::
cout
<<
"TopicSubscriber: subscription successfull: "
<<
clientName
<<
std
::
endl
;
}
}
src/comm/ros_bridge/include/
TopicS
ubscriber.h
→
src/comm/ros_bridge/include/
topic_s
ubscriber.h
View file @
2430ec45
#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/TypeFactory.h"
#include "ros_bridge/include/CasePacker.h"
namespace
ROSBridge
{
namespace
ComPrivate
{
#include <unordered_map>
namespace
ros_bridge
{
namespace
com_private
{
typedef
std
::
function
<
void
(
JsonDocUPtr
)
>
CallbackType
;
struct
MapStruct
{
typedef
std
::
map
<
HashType
,
CallbackType
>
Map
;
Map
map
;
std
::
mutex
mutex
;
};
class
TopicSubscriber
{
typedef
std
::
vector
<
std
::
string
>
ClientList
;
typedef
std
::
unordered_map
<
std
::
string
,
std
::
string
>
TopicMap
;
public:
TopicSubscriber
()
=
delete
;
TopicSubscriber
(
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
);
TopicSubscriber
(
RosbridgeWsClient
&
rbc
);
~
TopicSubscriber
();
//! @brief Starts the subscriber.
...
...
@@ -34,22 +29,13 @@ public:
//! @return Returns false if a subscription to this topic allready exists.
//!
//! @note Only one callback can be registered.
bool
subscribe
(
const
char
*
topic
,
const
CallbackType
&
callback
);
void
subscribe
(
const
char
*
topic
,
const
CallbackType
&
callback
);
private:
CasePacker
&
_casePacker
;
RosbridgeWsClient
&
_rbc
;
MapStruct
_callbackMapStruct
;
ClientList
_clientList
;
bool
_running
;
TopicMap
_topicMap
;
std
::
shared_ptr
<
std
::
atomic_bool
>
_stopped
;
};
void
subscriberCallback
(
const
HashType
&
hash
,
MapStruct
&
mapWrapper
,
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
);
}
// namespace ComPrivate
}
// namespace ROSBridge
src/comm/ros_bridge/src/CasePacker.cpp
deleted
100644 → 0
View file @
0e34b591
#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/PackageBuffer.h
deleted
100644 → 0
View file @
0e34b591
#pragma once
#include "boost/lockfree/queue.hpp"
#include <functional>
namespace
ROSBridge
{
namespace
Bridge
{
namespace
lf
=
::
boost
::
lockfree
;
template
<
class
T
>
class
PackageBuffer
{
public:
PackageBuffer
();
void
push
(
T
t
)
{
buffer
.
push
(
t
);
if
(
_pushCallback
)
_pushCallback
();
}
T
pop
()
{
T
temp
=
buffer
.
pop
();
if
(
_popCallback
)
_popCallback
();
return
temp
;
}
void
setPushCallback
(
std
::
function
<
void
(
void
)
>
pushCallback
)
{
_pushCallback
=
pushCallback
;
}
void
setPopCallback
(
std
::
function
<
void
(
void
)
>
popCallback
)
{
_popCallback
=
popCallback
;
}
bool
empty
()
const
{
return
buffer
.
empty
();}
private:
lf
::
queue
<
T
>
buffer
;
std
::
function
<
void
(
void
)
>
_popCallback
;
std
::
function
<
void
(
void
)
>
_pushCallback
;
};
}
// namespace Communicator
}
// namespace
src/comm/ros_bridge/src/ROSBridge.cpp
deleted
100644 → 0
View file @
0e34b591
#include "ros_bridge/include/ROSBridge.h"
ROSBridge
::
ROSBridge
::
ROSBridge
()
:
_casePacker
(
&
_typeFactory
,
&
_jsonFactory
)
,
_rbc
(
"localhost:9090"
)
,
_topicPublisher
(
_casePacker
,
_rbc
)
,
_topicSubscriber
(
_casePacker
,
_rbc
)
{
}
void
ROSBridge
::
ROSBridge
::
publish
(
ROSBridge
::
ROSBridge
::
JsonDocUPtr
doc
)
{
_topicPublisher
.
publish
(
std
::
move
(
doc
));
}
void
ROSBridge
::
ROSBridge
::
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
)
{
_topicSubscriber
.
subscribe
(
topic
,
callBack
);
}
const
ROSBridge
::
CasePacker
*
ROSBridge
::
ROSBridge
::
casePacker
()
const
{
return
&
_casePacker
;
}
void
ROSBridge
::
ROSBridge
::
start
()
{
_topicPublisher
.
start
();
_topicSubscriber
.
start
();
}
void
ROSBridge
::
ROSBridge
::
reset
()
{
_topicPublisher
.
reset
();
_topicSubscriber
.
reset
();
}
bool
ROSBridge
::
ROSBridge
::
connected
()
{
return
_rbc
.
connected
();
}
src/comm/ros_bridge/src/ros_bridge.cpp
0 → 100644
View file @
2430ec45
#include "ros_bridge/include/ros_bridge.h"
ros_bridge
::
ROSBridge
::
ROSBridge
(
const
char
*
connectionString
)
:
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_rbc
(
connectionString
,
false
/*run*/
)
,
_topicPublisher
(
_rbc
)
,
_topicSubscriber
(
_rbc
)
,
_server
(
_rbc
)
{
}
ros_bridge
::
ROSBridge
::
ROSBridge
()
:
ros_bridge
::
ROSBridge
::
ROSBridge
(
"localhost:9090"
)
{
}
void
ros_bridge
::
ROSBridge
::
publish
(
ros_bridge
::
ROSBridge
::
JsonDocUPtr
doc
,
const
char
*
topic
)
{
_topicPublisher
.
publish
(
std
::
move
(
doc
),
topic
);
}
void
ros_bridge
::
ROSBridge
::
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
)
{
_topicSubscriber
.
subscribe
(
topic
,
callBack
);
}
void
ros_bridge
::
ROSBridge
::
advertiseService
(
const
char
*
service
,
const
char
*
type
,
const
std
::
function
<
JsonDocUPtr
(
JsonDocUPtr
)
>
&
callback
)
{
_server
.
advertiseService
(
service
,
type
,
callback
);
}
void
ros_bridge
::
ROSBridge
::
advertiseTopic
(
const
char
*
topic
,
const
char
*
type
)
{
_topicPublisher
.
advertise
(
topic
,
type
);
}
void
ros_bridge
::
ROSBridge
::
start
()
{
if
(
!
_stopped
->
load
()
)
return
;
_stopped
->
store
(
false
);
_rbc
.
run
();
_topicPublisher
.
start
();
_topicSubscriber
.
start
();
_server
.
start
();
}
void
ros_bridge
::
ROSBridge
::
reset
()
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
if
(
_stopped
->
load
()
)
return
;
_stopped
->
store
(
true
);
_topicPublisher
.
reset
();
_topicSubscriber
.
reset
();
_server
.
reset
();
_rbc
.
reset
();
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
std
::
cout
<<
"ros_bridge reset duration: "
<<
delta
<<
" ms"
<<
std
::
endl
;
}
bool
ros_bridge
::
ROSBridge
::
connected
()
{
start
();
return
_rbc
.
connected
();
}
bool
ros_bridge
::
ROSBridge
::
isRunning
()
{
return
!
_stopped
->
load
();
}
bool
ros_bridge
::
isValidConnectionString
(
const
char
*
connectionString
)
{
return
is_valid_port_path
(
connectionString
);
}
src/comm/utilities.h
View file @
2430ec45
...
...
@@ -73,15 +73,4 @@ private:
bool
_isInitialized
;
};
template
<
typename
T
>
struct
Type2Type
{
typedef
T
OriginalType
;
};
template
<
int
k
>
struct
Int2Type
{
enum
{
value
=
k
};
};
#endif // UTILITIES_H
src/ui/preferences/GeneralSettings.qml
View file @
2430ec45
...
...
@@ -950,18 +950,18 @@ QGCView {
columns
:
4
QGCLabel
{
text
:
qsTr
(
"
Low
Battery Threshold
"
)
text
:
qsTr
(
"
Battery Threshold
"
)
visible
:
_userBrandImageIndoor
.
visible
}
FactTextField
{
Layout.preferredWidth
:
_valueFieldWidth
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
lowBatteryThreshold
Layout.columnSpan
:
2
}
FactCheckBox
{
text
:
"
Enable
low Battery handling
"
text
:
"
Enable
Smart RTL
"
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
enableLowBatteryHandling
Layout.columnSpan
:
2
}
QGCLabel
{
...
...
@@ -971,6 +971,20 @@ QGCView {
FactTextField
{
Layout.preferredWidth
:
_valueFieldWidth
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
minimalRemainingMissionTime
Layout.columnSpan
:
2
}
Item
{
// dummy
width
:
1
}
QGCLabel
{
text
:
qsTr
(
"
ROS Bridge Connection String
"
)
visible
:
_userBrandImageIndoor
.
visible
}
FactTextField
{
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
rosbridgeConnectionString
Layout.columnSpan
:
3
}
}
}
...
...
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