diff --git a/deploy/AppImageCommitHash b/deploy/AppImageCommitHash deleted file mode 100644 index 1b3e8c9891281ebe88b39f602a52d3537d2525ce..0000000000000000000000000000000000000000 --- a/deploy/AppImageCommitHash +++ /dev/null @@ -1 +0,0 @@ -bd79747292138b2ea0 diff --git a/deploy/QGroundControl.AppImage b/deploy/QGroundControl.AppImage new file mode 100755 index 0000000000000000000000000000000000000000..cae7c385e3bcf9b798ce7b85e70aeda7b69a439e Binary files /dev/null and b/deploy/QGroundControl.AppImage differ diff --git a/deploy/appImageCommand.txt b/deploy/appImageCommand.txt index 9fd6857014d5da0dc89c3c375f7b127aef6073d7..461808ffbec1880d550c60090fdbd9d37213188c 100644 --- a/deploy/appImageCommand.txt +++ b/deploy/appImageCommand.txt @@ -1 +1 @@ -./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 diff --git a/deploy/create_linux_appimage.sh b/deploy/create_linux_appimage.sh index 11883dec8c25d826c27452d2cace2e352430c2ed..4d1e7a9bd3b8c534b8a6dbf035d8cced027e62d7 100755 --- a/deploy/create_linux_appimage.sh +++ b/deploy/create_linux_appimage.sh @@ -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 diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 80ef0ac43bd03adbee1986759e3346464062ec8e..40feba8b9c0f250edace97a1a71b761240df344d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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/ROSBridge.cpp + src/comm/ros_bridge/src/ros_bridge.cpp # # Unit Test specific configuration goes here (requires full debug build with all plugins) diff --git a/src/Settings/Wima.SettingsGroup.json b/src/Settings/Wima.SettingsGroup.json index 9f9275c4f08b18a24173a7b5596c2126a0707e87..661453d2949f9fcec59c7ee41a6dbbfdfa95314c 100644 --- a/src/Settings/Wima.SettingsGroup.json +++ b/src/Settings/Wima.SettingsGroup.json @@ -18,5 +18,11 @@ "type": "uint64", "units": "s", "defaultValue": 15 +}, +{ + "name": "rosbridgeConnectionString", + "shortDescription": "Ros Bridge Connection String (host:port).", + "type": "string", + "defaultValue": "localhost:9090" } ] diff --git a/src/Settings/WimaSettings.cc b/src/Settings/WimaSettings.cc index 8aa5a8c575eef37628c9fdb515c470d546c86ac0..46a04fc79c14405654e3346bc839c7407e96cc02 100644 --- a/src/Settings/WimaSettings.cc +++ b/src/Settings/WimaSettings.cc @@ -11,3 +11,4 @@ DECLARE_SETTINGGROUP(Wima, "Wima") DECLARE_SETTINGSFACT(WimaSettings, lowBatteryThreshold) DECLARE_SETTINGSFACT(WimaSettings, enableLowBatteryHandling) DECLARE_SETTINGSFACT(WimaSettings, minimalRemainingMissionTime) +DECLARE_SETTINGSFACT(WimaSettings, rosbridgeConnectionString) diff --git a/src/Settings/WimaSettings.h b/src/Settings/WimaSettings.h index 3f2ab2e0c39f71d1f75eab26e71312cfd1a17667..30b747d67ef2f312b0a9b5d4264e5e3d15722a41 100644 --- a/src/Settings/WimaSettings.h +++ b/src/Settings/WimaSettings.h @@ -13,4 +13,5 @@ public: DEFINE_SETTINGFACT(lowBatteryThreshold) DEFINE_SETTINGFACT(enableLowBatteryHandling) DEFINE_SETTINGFACT(minimalRemainingMissionTime) + DEFINE_SETTINGFACT(rosbridgeConnectionString) }; diff --git a/src/Wima/Geometry/GeoPoint3D.cpp b/src/Wima/Geometry/GeoPoint3D.cpp index 2d75e61d312da1d10cc93db96f3ab79ea335f39c..2d5bd3d59366fa4e1c764f943e148dda65197651 100644 --- a/src/Wima/Geometry/GeoPoint3D.cpp +++ b/src/Wima/Geometry/GeoPoint3D.cpp @@ -1,30 +1,25 @@ #include "GeoPoint3D.h" -GeoPoint::GeoPoint(QObject *parent) +GeoPoint3D::GeoPoint3D(QObject *parent) : QObject(parent), ROSGeoPoint() {} -GeoPoint::GeoPoint(double latitude, double longitude, double altitude, QObject *parent) +GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent) : QObject(parent), ROSGeoPoint(latitude, longitude, altitude) {} -GeoPoint::GeoPoint(const GeoPoint &p, QObject *parent) +GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent) : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude()) {} -GeoPoint::GeoPoint(const ROSGeoPoint &p, QObject *parent) +GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent) : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude()) {} -GeoPoint::GeoPoint(const QGeoCoordinate &p, QObject *parent) +GeoPoint3D::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) +GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p) { this->setLatitude(p.latitude()); this->setLongitude(p.longitude()); diff --git a/src/Wima/Geometry/GeoPoint3D.h b/src/Wima/Geometry/GeoPoint3D.h index 5579b63b2ce9a7933c74017c76f0806820ae5521..63b221d117e52c061d27e3f48bcea27e3cc7f3f4 100644 --- a/src/Wima/Geometry/GeoPoint3D.h +++ b/src/Wima/Geometry/GeoPoint3D.h @@ -1,37 +1,31 @@ #pragma once -#include "ros_bridge/include/JsonMethodes.h" -#include "ros_bridge/include/MessageBaseClass.h" -#include "ros_bridge/include/GenericMessages.h" - #include #include +#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, + GeoPoint3D(QObject *parent = nullptr); + GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent = nullptr); - GeoPoint(const GeoPoint& p, + GeoPoint3D(const GeoPoint3D& p, QObject *parent = nullptr); - GeoPoint(const ROSGeoPoint& p, + GeoPoint3D(const ROSGeoPoint& p, QObject *parent = nullptr); - GeoPoint(const QGeoCoordinate& p, + GeoPoint3D(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); }; diff --git a/src/Wima/Geometry/Polygon2D.h b/src/Wima/Geometry/Polygon2D.h index 4ced5c058f42ce6b5e3dda53b024a0c11e15de0b..d9390a5ad4cc795572dfb558eeb2f025e22ceddd 100644 --- a/src/Wima/Geometry/Polygon2D.h +++ b/src/Wima/Geometry/Polygon2D.h @@ -3,41 +3,30 @@ #include #include -#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 ContainerType = QVector> -class Polygon2DTemplate : public ROSMsg{ // - typedef ROSBridge::GenericMessages::GeometryMsgs::GenericPolygon Poly; +class Polygon2DTemplate{ // + typedef ros_bridge::messages::geometry_msgs::polygon::GenericPolygon 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 Polygon &polygon() const {return _polygon;} + Polygon &polygon() {return _polygon;} const ContainerType &path() const {return _polygon.points();} ContainerType &path() {return _polygon.points();} - virtual Polygon2DTemplate*Clone() const { // ROSMsg - return new Polygon2DTemplate(*this); - } private: - Poly _polygon; + Polygon _polygon; }; diff --git a/src/Wima/Geometry/PolygonArray.h b/src/Wima/Geometry/PolygonArray.h index 9a658169fbd6718c374e44bece23e58bb5ffd56b..3d92547e2a4541f1db50bcaa65cbd6fce7348830 100644 --- a/src/Wima/Geometry/PolygonArray.h +++ b/src/Wima/Geometry/PolygonArray.h @@ -1,14 +1,11 @@ #pragma once -#include +#include -#include "ros_bridge/include/MessageBaseClass.h" - -typedef ROSBridge::MessageBaseClass ROSMsgBase; template class ContainerType > -class PolygonArray : public ROSMsgBase, public ContainerType { +class PolygonArray : public ContainerType { public: - explicit PolygonArray() : ROSMsgBase(), ContainerType() {} + explicit PolygonArray() : ContainerType() {} PolygonArray(const PolygonArray &other) : ContainerType(other) {} QString type() const override {return "PolygonArray";} diff --git a/src/Wima/Geometry/WimaPolygonArray.h b/src/Wima/Geometry/WimaPolygonArray.h index 3815899e46b82127bc359e28f0bcf95213a5a75e..359bf8c4667f0ab386040686a542c6fd0b40441f 100644 --- a/src/Wima/Geometry/WimaPolygonArray.h +++ b/src/Wima/Geometry/WimaPolygonArray.h @@ -1,28 +1,20 @@ #pragma once -#include "ros_bridge/include/MessageBaseClass.h" -#include "ros_bridge/include/MessageGroups.h" #include "QmlObjectListModel.h" #include #include -typedef ROSBridge::MessageBaseClass ROSMsg; -namespace MsgGroups = ROSBridge::MessageGroups; -typedef MsgGroups::EmptyGroup EmptyGroup; -template class ContainerType = QVector, typename GroupType = EmptyGroup> -class WimaPolygonArray : public ROSMsg +template 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.clearAndDeleteContents(); for (long i=0; i<_polygons.size(); ++i){ - _objs.append(&_polygons[i]); + _objs.append(new PolygonType(_polygons[i])); } } diff --git a/src/Wima/Snake/PolygonArray.h b/src/Wima/Snake/PolygonArray.h index 48df733ee9d89c4e3338a72689f1a158da1aed0f..ceebbacd9f0ef7fd003ee3831ba13cdd9f2d165c 100644 --- a/src/Wima/Snake/PolygonArray.h +++ b/src/Wima/Snake/PolygonArray.h @@ -1,8 +1,6 @@ #pragma once -#include "ros_bridge/include/MessageGroups.h" #include "Wima/Geometry/Polygon2D.h" #include "Wima/Geometry/WimaPolygonArray.h" -namespace MsgGroups = ROSBridge::MessageGroups; -typedef WimaPolygonArray PolygonArray; +typedef WimaPolygonArray SnakeTilesLocal; diff --git a/src/Wima/Snake/QNemoHeartbeat.h b/src/Wima/Snake/QNemoHeartbeat.h index 840ab74e3280d4171d9abd148934f4d0af5a9683..8e95986eb6324c4657d91ecc5965ccbf7e4cd24d 100644 --- a/src/Wima/Snake/QNemoHeartbeat.h +++ b/src/Wima/Snake/QNemoHeartbeat.h @@ -1,5 +1,5 @@ #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; diff --git a/src/Wima/Snake/QNemoProgress.h b/src/Wima/Snake/QNemoProgress.h index 1f2d5e037b9197cbd64166925af954a371c59b3e..e57ec2fdf108993dff80f7d5544b6d10a77c39c2 100644 --- a/src/Wima/Snake/QNemoProgress.h +++ b/src/Wima/Snake/QNemoProgress.h @@ -1,10 +1,8 @@ #pragma once -#include -#include - -#include "ros_bridge/include/GenericMessages.h" -namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs; -typedef NemoMsgs::GenericProgress QNemoProgress; +#include +#include "ros_bridge/include/messages/nemo_msgs/progress.h" +namespace nemo = ros_bridge::messages::nemo_msgs; +typedef nemo::progress::GenericProgress QNemoProgress; diff --git a/src/Wima/Snake/QtROSJsonFactory.h b/src/Wima/Snake/QtROSJsonFactory.h deleted file mode 100644 index a58386e3f8afa90d30ae3f823cd58a6d00366f08..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/QtROSJsonFactory.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once -#include "ros_bridge/include/JsonFactory.h" -#include - -typedef ROSBridge::GenericJsonFactory<> QtROSJsonFactory; diff --git a/src/Wima/Snake/QtROSTypeFactory.h b/src/Wima/Snake/QtROSTypeFactory.h deleted file mode 100644 index a2e43383d605650c46413209ae992b48044a527e..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/QtROSTypeFactory.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once -#include "ros_bridge/include/TypeFactory.h" -#include - -typedef ROSBridge::TypeFactory QtROSTypeFactory; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 24bf234c533a7ba4cdcdc17125df9144ab177d13..c3bc50d6201ea12dbf7d59a2f7ce833770b33193 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -1,22 +1,29 @@ #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 +#include + +#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,8 +64,9 @@ WimaController::WimaController(QObject *parent) , _rtlManager (_managerSettings, _areaInterface) , _currentManager (&_defaultManager) , _managerList {&_defaultManager, &_snakeManager, &_rtlManager} - , _metaDataMap (FactMetaData::createMapFromJsonFile( - QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) + , _metaDataMap ( + FactMetaData::createMapFromJsonFile( + QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) , _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName]) , _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName]) , _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName]) @@ -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, &SnakeDataManager::finished, this, &WimaController::_updateSnakeData); + connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults); connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); - connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeDataManager::quit); + connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::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()}; + mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); + } + mArea.type = AreaType::MeasurementArea; + + Area sArea; + for (auto variant : _serviceArea.path()){ + QGeoCoordinate c{variant.value()}; + sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); + } + sArea.type = AreaType::ServiceArea; + + Area corridor; + for (auto variant : _corridor.path()){ + QGeoCoordinate c{variant.value()}; + 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 ¢erPoint = 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::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::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(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(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(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(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(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) { - _pRosBridge->reset(); - _bridgeSetupDone = false; + } else if ( _pRosBridge->isRunning() ) { + _pRosBridge->reset(); + _topicServiceSetupDone = 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)]); - } - auto end = std::chrono::high_resolution_clock::now(); - double duration = std::chrono::duration_cast(end-start).count(); - qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms."; - + _snakeManager.push_back(r.waypoints[int(i)].value()); + } - start = std::chrono::high_resolution_clock::now(); _snakeManager.update(); - end = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(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(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(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(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(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::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::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::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::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::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::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; +} diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index c2e49dcfadbcbb1741ad839efad1837ac8a3dbe5..924467b8b74704d1eaddde597b8d7997590c72f0 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -35,18 +35,14 @@ #include "Snake/QNemoProgress.h" #include "Snake/QNemoHeartbeat.h" -#include "ros_bridge/include/ROSBridge.h" +#include "ros_bridge/include/ros_bridge.h" #include "WaypointManager/DefaultManager.h" #include "WaypointManager/RTLManager.h" -#include +#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 using namespace snake; @@ -59,7 +55,7 @@ class WimaController : public QObject enum FileType {WimaFile, PlanFile}; - typedef QScopedPointer ROSBridgePtr; + typedef QScopedPointer 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; + QTimer _eventTimer; + EventTicker _batteryLevelTicker; + EventTicker _snakeTicker; + EventTicker _nemoTimeoutTicker; }; diff --git a/src/comm/ros_bridge/include/CasePacker.h b/src/comm/ros_bridge/include/CasePacker.h deleted file mode 100644 index 3bf889a744f8f67a2a065fdfcfae94fe6b1e30cc..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/CasePacker.h +++ /dev/null @@ -1,54 +0,0 @@ -#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 -#include "rapidjson/include/rapidjson/document.h" - -namespace ROSBridge { - -class CasePacker -{ - typedef MessageTag Tag; - typedef rapidjson::Document JsonDoc; - typedef std::unique_ptr JsonDocUPtr; -public: - CasePacker() = delete; - CasePacker(TypeFactory *typeFactory, JsonFactory *jsonFactory); - - template - 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 - 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; -}; -} - diff --git a/src/comm/ros_bridge/include/ComPrivateInclude.cpp b/src/comm/ros_bridge/include/ComPrivateInclude.cpp deleted file mode 100644 index ac583a76d1f01f85b51d1ac5a28048656b428977..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/ComPrivateInclude.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "ros_bridge/include/ComPrivateInclude.h" -#include - -std::size_t ROSBridge::ComPrivate::getHash(const std::string &str) -{ - std::hash hash; - return hash(str); -} - -std::size_t ROSBridge::ComPrivate::getHash(const char *str) -{ - return ROSBridge::ComPrivate::getHash(std::string(str)); -} diff --git a/src/comm/ros_bridge/include/GenericMessages.h b/src/comm/ros_bridge/include/GenericMessages.h deleted file mode 100644 index 7e19254cd27b243f9574cddb217849a034a070d9..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/GenericMessages.h +++ /dev/null @@ -1,371 +0,0 @@ -#pragma once -#include -#include - -#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 -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 ContainerType = std::vector> -class GenericPolygon : public ROSBridge::MessageBaseClass { - typedef typename boost::remove_cv_ref_t PointType; - typedef ROSBridge::MessageBaseClass Base; -public: - typedef ROSBridge::MessageGroups::PolygonGroup Group; - GenericPolygon() : Base() {} - GenericPolygon(const GenericPolygon &poly) : Base(), _points(poly.points()){} - - - const ContainerType &points() const {return _points;} - ContainerType &points() {return _points;} - - GenericPolygon *Clone() const override { return new GenericPolygon(*this);} - - GenericPolygon &operator=(const GenericPolygon &other) { - this->_points = other._points; - return *this; - } - -private: - ContainerType _points; -}; - -typedef GenericPolygon Polygon; - - -// ============================================================================== -// class GenericPolygonStamped -// ============================================================================== -using namespace ROSBridge::GenericMessages::StdMsgs; - -//! @brief C++ representation of geometry_msgs/PolygonStamped -template -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 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 &polygons, - const ContainerType &labels, - const ContainerType &likelihood) - : Base() - , _header(header) - , _polygons(polygons) - , _labels(labels) - , _likelihood(likelihood) - {} - - const HeaderType &header() const {return _header;} - HeaderType &header() {return _header;} - const ContainerType &polygons() const {return _polygons;} - ContainerType &polygons() {return _polygons;} - const ContainerType &labels() const {return _labels;} - ContainerType &labels() {return _labels;} - const ContainerType &likelyhood() const {return _likelihood;} - ContainerType &likelyhood() {return _likelihood;} - - GenericPolygonArray *Clone() const override {return new GenericPolygonArray(*this);} - - -private: - HeaderType _header; - ContainerType _polygons; - ContainerType _labels; - ContainerType _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 ContainterType = std::vector> -class GenericProgress : public MessageBaseClass{ -public: - typedef MessageGroups::ProgressGroup Group; - GenericProgress(){} - GenericProgress(const ContainterType &progress) :_progress(progress){} - GenericProgress(const GenericProgress &p) :_progress(p.progress()){} - ~GenericProgress() {} - - virtual GenericProgress *Clone() const override { return new GenericProgress(*this); } - - virtual const ContainterType &progress(void) const {return _progress;} - virtual ContainterType &progress(void) {return _progress;} -protected: - ContainterType _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 diff --git a/src/comm/ros_bridge/include/JsonFactory.h b/src/comm/ros_bridge/include/JsonFactory.h deleted file mode 100644 index e3e1d623789cfeedc8fc06e63e27839010fcb137..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/JsonFactory.h +++ /dev/null @@ -1,238 +0,0 @@ -#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 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 - rapidjson::Document *create(const T &msg){ - static_assert(boost::is_base_of::value, "Type of msg must be derived from MessageBaseClass."); - static_assert(!::boost::is_same::value, - "msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)"); - //cout << T::Group::label() << endl; - return _create(msg, Type2Type()); - } - -private: - - // =========================================================================== - // EmptyGroup and not implemented Groups - // =========================================================================== - template - rapidjson::Document *_create(const U &msg, Type2Type){ - (void)msg; - assert(false); // Implementation missing for group U::Group! - return nullptr; - } - - // =========================================================================== - // Point32Group - // =========================================================================== - template - rapidjson::Document *_create(const U &msg, Type2Type){ - 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 - rapidjson::Document *_create(const U &msg, Type2Type){ - 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 - rapidjson::Document *_create(const U &msg, Type2Type){ - 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 - rapidjson::Document *_create(const U &msg, Type2Type){ - using namespace ROSBridge; - using namespace ROSBridge::traits; - typedef HasMemberHeader HasHeader; - - return _createPolygonStamped(msg, Int2Type()); - } - - template - rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type){ // 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 - 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 - rapidjson::Document *_create(const U &msg, Type2Type){ - using namespace ROSBridge; - using namespace ROSBridge::traits; - typedef HasMemberHeader HasHeader; - - return _createPolygonArray(msg, Int2Type()); - } - - template - rapidjson::Document *_createPolygonArray(const U &msg, Int2Type){ // 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 - 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 - rapidjson::Document *_create(const U &msg, Type2Type){ - 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 - rapidjson::Document *_create(const U &msg, Type2Type){ - 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 - 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 - Time time(const T &msg) { - (void)msg; - return Time(0,0); - } - -private: - long _seq; -}; - -typedef GenericJsonFactory<> JsonFactory; - -} // end namespace ros_bridge - - diff --git a/src/comm/ros_bridge/include/JsonMethodes.h b/src/comm/ros_bridge/include/JsonMethodes.h deleted file mode 100644 index 9444a4c3379392bac5e7e103ce2bad2cf2620806..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/JsonMethodes.h +++ /dev/null @@ -1,695 +0,0 @@ -#ifndef MESSAGES_H -#define MESSAGES_H - -#include -#include -#include -#include - -#include "ros_bridge/rapidjson/include/rapidjson/rapidjson.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" - -#include "utilities.h" -#include "MessageTraits.h" -#include - -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 - 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 - 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 - 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 - 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 - auto getZ(const T &p, Type2Type) - { - return p.z(); - } - - template - auto getZ(const T &p, Type2Type) - { - (void)p; - return 0.0; // p has no member z() -> add 0. - } - - template - bool setZ(const rapidjson::Value &doc, const T &p, Type2Type) - { - p.setZ(doc["z"].GetFloat()); - return true; - } - - template - bool setZ(const rapidjson::Value &doc, const T &p, Type2Type) - { - (void)doc; - (void)p; - return true; - } - } - - template - 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::value, Has3Components, Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - auto z = det::getZ(p, Type2Type()); // If T has no member z() replace it by 0. - - value.AddMember("z", rapidjson::Value().SetFloat(z), allocator); - - return true; - } - - template - 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::value, Has3Components, Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - (void)det::setZ(value["z"], p, Type2Type()); // 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 -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 -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::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 -bool toJson(const PolyStamped &polyStamped, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) -{ - return toJson(polyStamped.polygon(), polyStamped.header(), value, allocator); -} - -template -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 -bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<1>) { // polyStamped.setHeader() exists - typedef decltype (polyStamped.header()) HeaderTypeCVR; - typedef typename boost::remove_cv::type>::type HeaderType; - HeaderType header; - bool ret = Header::fromJson(doc, header); - polyStamped.header() = header; - return ret; -} -template -bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<0>) { // polyStamped.setHeader() does not exists - (void)doc; - (void)polyStamped; - return true; -} -} // namespace det - -template -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 HasHeader; - if ( !det::setHeader(value["header"], polyStamped, Int2Type())){ - 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 - auto getAltitude(const T &p, Type2Type) - { - return p.altitude(); - } - - template - auto getAltitude(const T &p, Type2Type) - { - (void)p; - return 0.0; - } - - template - void setAltitude(const rapidjson::Value &doc, T &p, Type2Type) - { - p.setAltitude(doc.GetFloat()); - } - - template - void setAltitude(const rapidjson::Value &doc, T &p, Type2Type) - { - (void)doc; - (void)p; - } - } // namespace det - - template - 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::value, Has3Components, Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - auto altitude = det::getAltitude(p, Type2Type()); // If T has no member altitude() replace it by 0.0; - - value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator); - return true; - } - - - - template - 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::value, Has3Components, Has2Components>::Result - Components; // Check if PointType has 2 or 3 dimensions. - det::setAltitude(value["altitude"], p, Type2Type()); // 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 - void labelsToJson(const PolygonArrayType &p, rapidjson::Value &labels, rapidjson::Document::AllocatorType &allocator, Int2Type){ - 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 - 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 - void likelihoodToJson(const PolygonArrayType &p, - rapidjson::Value &likelyhood, - rapidjson::Document::AllocatorType &allocator, - Int2Type){ - 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 - 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 - void setLabels(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type){ - 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 - void setLabels(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<0>){ - (void)doc; - (void)p; - } - - //! \note \p p has member \fn likelihood(). - template - void setLikelihood(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type){ - 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 - 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 - 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 HasLabels; - PAdetail::labelsToJson(pArray, labels, allocator, Int2Type()); - value.AddMember("labels", labels, allocator); - - - rapidjson::Value likelihood(rapidjson::kArrayType); - typedef HasMemberLikelihood HasLikelihood; - PAdetail::likelihoodToJson(pArray, likelihood, allocator, Int2Type()); - 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 - 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 HasLabels; - PAdetail::labelsToJson(p, labels, allocator, Int2Type()); - value.AddMember("labels", labels, allocator); - - - rapidjson::Value likelihood(rapidjson::kArrayType); - typedef HasMemberLikelihood HasLikelihood; - PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type()); - value.AddMember("likelihood", likelihood, allocator); - - return true; - } - - template - 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 HasHeader; - if ( !PolygonStamped::det::setHeader(value["header"], p, Int2Type())){ - 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::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())){ - 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 HasLabels; - PAdetail::setLabels(value["labels"], p, Int2Type()); - - typedef traits::HasMemberLikelihood HasLikelihood; - PAdetail::setLikelihood(value["likelihood"], p, Int2Type()); - - 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 - 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 - 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 - bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) - { - value.AddMember("status", std::int32_t(heartbeat.status()), allocator); - return true; - } - - template - 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 diff --git a/src/comm/ros_bridge/include/MessageBaseClass.h b/src/comm/ros_bridge/include/MessageBaseClass.h deleted file mode 100644 index b2d032c73faae8183b515336f96683bb7d38fcd6..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/MessageBaseClass.h +++ /dev/null @@ -1,30 +0,0 @@ -#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 - - diff --git a/src/comm/ros_bridge/include/MessageGroups.h b/src/comm/ros_bridge/include/MessageGroups.h deleted file mode 100644 index 7b95eef76aa215455731100b6007595b588266bb..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/MessageGroups.h +++ /dev/null @@ -1,158 +0,0 @@ -#pragma once - -#include - -namespace ROSBridge { - -namespace MessageGroups { - -typedef std::string StringType; - -template -struct MessageGroup { - static StringType messageType() {return _full();} - - template - static StringType _full() {return G::label()+ "/" + _full(); } - template - static StringType _full() {return G::label(); } - - - static StringType messageTypeLast() {return _last();} - - template - static StringType _last() {return _last(); } - template - 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 - EmptyGroup; - -typedef MessageGroups::MessageGroup - Point32Group; - -typedef MessageGroups::MessageGroup - GeoPointGroup; - -typedef MessageGroups::MessageGroup - PolygonGroup; - -typedef MessageGroups::MessageGroup - PolygonStampedGroup; - -typedef MessageGroups::MessageGroup - PolygonArrayGroup; - -typedef MessageGroups::MessageGroup - ProgressGroup; - -typedef MessageGroups::MessageGroup - HeartbeatGroup; - -} // end namespace MessageGroups - -} // end namespace ros_bridge diff --git a/src/comm/ros_bridge/include/MessageTag.cpp b/src/comm/ros_bridge/include/MessageTag.cpp deleted file mode 100644 index 80a74d9c99b0100737707282d2fcc9b4a9fd621a..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/MessageTag.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#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; -} diff --git a/src/comm/ros_bridge/include/MessageTag.h b/src/comm/ros_bridge/include/MessageTag.h deleted file mode 100644 index cc04dc6b80302b6a9bfc91dc85367dd31500dfe2..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/MessageTag.h +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include - - -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; diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h deleted file mode 100644 index 6ee6f94e9e15cb2969ab876b87b53a82c29fd199..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/ROSBridge.h +++ /dev/null @@ -1,52 +0,0 @@ -#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 -#include - -#include "boost/lockfree/queue.hpp" - -namespace ROSBridge { -class ROSBridge -{ -public: - typedef MessageTag Tag; - typedef rapidjson::Document JsonDoc; - typedef std::unique_ptr JsonDocUPtr; - - explicit ROSBridge(); - - template - void publish(T &msg, const std::string &topic){ - _topicPublisher.publish(msg, topic); - } - void publish(JsonDocUPtr doc); - - void subscribe(const char *topic, const std::function &callBack); - - const CasePacker *casePacker() const; - - //! @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. - bool connected(); - -private: - TypeFactory _typeFactory; - JsonFactory _jsonFactory; - CasePacker _casePacker; - RosbridgeWsClient _rbc; - ComPrivate::TopicPublisher _topicPublisher; - ComPrivate::TopicSubscriber _topicSubscriber; - -}; -} diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3a44e7766a88c6420ff8e98b7f1b315db8557426 --- /dev/null +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -0,0 +1,970 @@ +#include "RosBridgeClient.h" + + +#include +#include +#include +#include +#include + + +struct Task{ + std::function ready; // Condition under which command should be executed. + std::function execute; // Command to execute. + std::function expired; // Returns true if the command is expired. + std::function clear_up; // operation to perform if task expired. + std::string name; +}; + +void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr 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 connection) { +#else + client->on_open = [message](std::shared_ptr 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 /*connection*/, std::shared_ptr 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 /*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 /*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(false)) + , stopped(std::make_shared(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 ([this] { + std::list 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(false); + auto status_client = std::make_shared(this->server_port_path); + status_client->on_open = [status_set, this](std::shared_ptr) { + 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(false); + this->callService("/rosapi/topics", [topics_set, this]( + std::shared_ptr connection, + std::shared_ptr in_message){ + std::unique_lock 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 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(false); + this->callService("/rosapi/services", [this, services_set]( + std::shared_ptr connection, + std::shared_ptr in_message){ + std::unique_lock 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 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 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 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 lk(mutex); + std::unordered_map>::iterator it = client_map.find(client_name); + if (it == client_map.end()) + { + client_map[client_name] = std::make_shared(server_port_path); + } +#ifdef ROS_BRIDGE_DEBUG + else + { + std::cerr << client_name << " has already been created" << std::endl; + } +#endif +} + +std::shared_ptr RosbridgeWsClient::getClient(const std::string &client_name) +{ + std::lock_guard lk(mutex); + std::unordered_map>::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 lk(mutex); + std::unordered_map>::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 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 lk(mutex); + std::unordered_map>::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 lk(mutex); + return available_topics; +} + +std::string RosbridgeWsClient::getAdvertisedServices(){ + std::lock_guard lk(mutex); + return available_services; +} + +bool RosbridgeWsClient::topicAvailable(const std::string &topic){ + std::lock_guard 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 lk(mutex); + std::unordered_map>::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(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 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 connection) { +#else + client->on_open = [this, topic, message](std::shared_ptr 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 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(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(server_port_path); +#ifdef ROS_BRIDGE_DEBUG + client->on_open = [client_name, message](std::shared_ptr connection) { +#else + client->on_open = [message](std::shared_ptr 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(entry) == EntryType::AdvertisedTopic ){ + unadvertise(std::get(entry)); + } + } +} + +void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Document &msg, const std::string &id) +{ + std::lock_guard 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(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 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 publish_client = std::make_shared(server_port_path); +#ifdef ROS_BRIDGE_DEBUG + publish_client->on_open = [message, client_name](std::shared_ptr connection) { +#else + publish_client->on_open = [message, client_name](std::shared_ptr 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 lk(mutex); + std::unordered_map>::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(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 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 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(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(server_port_path); +#ifdef ROS_BRIDGE_DEBUG + client->on_open = [client_name, message](std::shared_ptr connection) { +#else + client->on_open = [message](std::shared_ptr 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(entry) == EntryType::SubscribedTopic ) { + unsubscribe(std::get(entry)); + } + } +} + +void RosbridgeWsClient::advertiseService(const std::string &client_name, const std::string &service, const std::string &type, const InMessage &callback) +{ + std::lock_guard lk(mutex); + std::unordered_map>::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(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 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 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(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(server_port_path); +#ifdef ROS_BRIDGE_DEBUG + client->on_open = [client_name, message](std::shared_ptr connection) { +#else + client->on_open = [message](std::shared_ptr 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(entry) == EntryType::AdvertisedService ) { + unadvertiseService(std::get(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 writer(strbuf); + values.Accept(writer); + + message += ", \"values\":" + std::string(strbuf.GetString()); + message = "{" + message + "}"; + + std::string client_name = "service_response_client" + service; + std::shared_ptr service_response_client = std::make_shared(server_port_path); + +#ifdef ROS_BRIDGE_DEBUG + service_response_client->on_open = [message, client_name](std::shared_ptr connection) { +#else + service_response_client->on_open = [message](std::shared_ptr 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 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 call_service_client = std::make_shared(server_port_path); + + if (callback) + { + call_service_client->on_message = callback; + } + else + { + call_service_client->on_message = [client_name](std::shared_ptr connection, std::shared_ptr 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 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 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(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 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(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); +} diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index 83e6febe0fe31401e6af2b1ccd3d91c197cd0a5c..251922941a1bbb2fc5207300557fa058044201a7 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -11,400 +11,153 @@ #include "rapidjson/include/rapidjson/writer.h" #include "rapidjson/include/rapidjson/stringbuffer.h" -#include #include -#include -#include #include +#include +#include +#include + +bool is_valid_port_path(std::string server_port_path); using WsClient = SimpleWeb::SocketClient; using InMessage = std::function, std::shared_ptr)>; +template +constexpr typename std::underlying_type::type integral(T value) +{ + return static_cast::type>(value); +} + class RosbridgeWsClient { - std::string server_port_path; - std::unordered_map> client_map; - std::mutex map_mutex; - - void start(const std::string& client_name, std::shared_ptr client, const std::string& message) - { - if (!client->on_open) - { -#ifdef DEBUG - client->on_open = [client_name, message](std::shared_ptr connection) { -#else - client->on_open = [message](std::shared_ptr 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); - }; - } - -#ifdef DEBUG - if (!client->on_message) - { - client->on_message = [client_name](std::shared_ptr /*connection*/, std::shared_ptr 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 /*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 /*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(); - } + enum class EntryType{ + SubscribedTopic, + AdvertisedTopic, + AdvertisedService, + }; -public: - RosbridgeWsClient(const std::string& server_port_path) - { - this->server_port_path = server_port_path; - } - - ~RosbridgeWsClient() - { - std::lock_guard 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>(); - auto future = p->get_future(); - - auto callback = [](std::shared_ptr connection, std::shared_ptr> p) { - p->set_value(); - connection->send_close(1000); + using EntryData = std::tuple /*client*/>; + + enum class EntryEnum{ + EntryType = 0, + ServiceTopicName = 1, + ClientName = 2, + WPClient = 3 }; - std::shared_ptr status_client = std::make_shared(server_port_path); - status_client->on_open = std::bind(callback, std::placeholders::_1, p); - std::async([status_client]{ + const std::string server_port_path; + std::unordered_map /*client*/> client_map; + std::deque service_topic_list; + std::mutex mutex; + std::shared_ptr isConnected; + std::shared_ptr stopped; + std::string available_topics; + std::string available_services; + std::shared_ptr periodic_thread; + + + + void start(const std::string& client_name, std::shared_ptr client, const std::string& message); + +public: + RosbridgeWsClient(const std::string& server_port_path); + RosbridgeWsClient(const std::string& server_port_path, bool run); - status_client->start(); - status_client->on_open = NULL; - status_client->on_message = NULL; - status_client->on_close = NULL; - status_client->on_error = NULL; + ~RosbridgeWsClient(); - }); + bool connected(); - auto status = future.wait_for(std::chrono::milliseconds(100)); - return status == std::future_status::ready; - } + void run(); + void stop(); + void reset(); // Adds a client to the client_map. - void addClient(const std::string& client_name) - { - std::lock_guard lk(map_mutex); - std::unordered_map>::iterator it = client_map.find(client_name); - if (it == client_map.end()) - { - client_map[client_name] = std::make_shared(server_port_path); - } -#ifdef DEBUG - else - { - std::cerr << client_name << " has already been created" << std::endl; - } -#endif - } - - std::shared_ptr getClient(const std::string& client_name) - { - std::lock_guard lk(map_mutex); - std::unordered_map>::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 lk(map_mutex); - std::unordered_map>::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 lk(map_mutex); - std::unordered_map>::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 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 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 lk(map_mutex); - std::unordered_map>::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 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 publish_client = std::make_shared(server_port_path); - - publish_client->on_open = [message](std::shared_ptr 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 lk(map_mutex); - std::unordered_map>::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 lk(map_mutex); - std::unordered_map>::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 writer(strbuf); - values.Accept(writer); - - message += ", \"values\":" + std::string(strbuf.GetString()); - message = "{" + message + "}"; - - std::shared_ptr service_response_client = std::make_shared(server_port_path); - - service_response_client->on_open = [message](std::shared_ptr 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 = ""); + + 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("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 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 call_service_client = std::make_shared(server_port_path); - - if (callback) - { - call_service_client->on_message = callback; - } - else - { - call_service_client->on_message = [](std::shared_ptr connection, std::shared_ptr 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); - }; - } - - 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 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 wait_for_service_client = std::make_shared(server_port_path); - std::shared_ptr> p(std::make_shared>()); - 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 connection, - std::shared_ptr 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(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 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 stop); }; diff --git a/src/comm/ros_bridge/include/ThreadSafeQueue.h b/src/comm/ros_bridge/include/ThreadSafeQueue.h deleted file mode 100644 index 03d653eeeacf2f6be8bb6a8c84c830054d9daaf5..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/ThreadSafeQueue.h +++ /dev/null @@ -1,79 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace ROSBridge { - -template -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 _queue; - std::mutex _mutex; - std::condition_variable _cond; -}; - -template -ThreadSafeQueue::ThreadSafeQueue(){} - -template -ThreadSafeQueue::~ThreadSafeQueue(){} - -template -T ThreadSafeQueue::pop_front() -{ - std::unique_lock mlock(_mutex); - while (_queue.empty()) - { - _cond.wait(mlock); - } - T t = std::move(_queue.front()); - _queue.pop_front(); - return t; -} - -template -void ThreadSafeQueue::push_back(const T& item) -{ - std::unique_lock mlock(_mutex); - _queue.push_back(item); - mlock.unlock(); // unlock before notificiation to minimize mutex con - _cond.notify_one(); // notify one waiting thread - -} - -template -void ThreadSafeQueue::push_back(T&& item) -{ - std::unique_lock 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 -int ThreadSafeQueue::size() -{ - std::unique_lock mlock(_mutex); - int size = _queue.size(); - mlock.unlock(); - return size; -} - -} // namespace - diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp deleted file mode 100644 index 1e44a7a74804f465ca7f2f9365dc450bb26d664e..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include "TopicPublisher.h" - - - - - -struct ROSBridge::ComPrivate::ThreadData -{ - const ROSBridge::CasePacker &casePacker; - RosbridgeWsClient &rbc; - ROSBridge::ComPrivate::JsonQueue &queue; - std::mutex &queueMutex; - const std::atomic &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(&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 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); -} - diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/TopicPublisher.h deleted file mode 100644 index 1c9dc680aec09f50ba5d9a64c3185e7d00ec6b07..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/TopicPublisher.h +++ /dev/null @@ -1,67 +0,0 @@ -#pragma once - -#include "ros_bridge/include/TypeFactory.h" -#include "ros_bridge/include/CasePacker.h" -#include "ros_bridge/include/ComPrivateInclude.h" -#include "ros_bridge/include/RosBridgeClient.h" - -#include -#include -#include -#include -#include -#include - -namespace ROSBridge { -namespace ComPrivate { - -struct ThreadData; - -class TopicPublisher -{ - typedef std::unique_ptr ThreadPtr; - using CondVar = std::condition_variable; -public: - - TopicPublisher() = delete; - TopicPublisher(CasePacker &casePacker, - RosbridgeWsClient &rbc); - - ~TopicPublisher(); - - //! @brief Starts the publisher. - void start(); - - //! @brief Resets the publisher. - void reset(); - - void publish(JsonDocUPtr docPtr){ - { - std::lock_guard lock(_queueMutex); - _queue.push_back(std::move(docPtr)); - } - _cv.notify_one(); // Wake publisher thread. - } - - template - void publish(const T &msg, const std::string &topic){ - JsonDocUPtr docPtr(_casePacker.pack(msg, topic)); - publish(std::move(docPtr)); - } - -private: - JsonQueue _queue; - std::mutex _queueMutex; - std::atomic _running; - CasePacker &_casePacker; - RosbridgeWsClient &_rbc; - CondVar _cv; - ThreadPtr _pThread; -}; - - -void transmittLoop(ThreadData data); - - -} // namespace CommunicatorDetail -} // namespace ROSBridge diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/TopicSubscriber.cpp deleted file mode 100644 index 7a1d8311fbdfa364ec6bf22c4413ed075d1d2aa8..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/TopicSubscriber.cpp +++ /dev/null @@ -1,136 +0,0 @@ -#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 lk(_callbackMapStruct.mutex); - _callbackMapStruct.map.clear(); - } - _clientList.clear(); - } -} - -bool ROSBridge::ComPrivate::TopicSubscriber::subscribe( - const char *topic, - const std::function &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 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(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(end-start).count() - << " ms" << std::endl; - } - - return true; -} - - -using WsClient = SimpleWeb::SocketClient; -void ROSBridge::ComPrivate::subscriberCallback( - const HashType &hash, - ROSBridge::ComPrivate::MapStruct &mapWrapper, - std::shared_ptr, - std::shared_ptr 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 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; - -} diff --git a/src/comm/ros_bridge/include/TopicSubscriber.h b/src/comm/ros_bridge/include/TopicSubscriber.h deleted file mode 100644 index 585e364771181201657952739438a5fea7b9653b..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/TopicSubscriber.h +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include "ros_bridge/include/ComPrivateInclude.h" -#include "ros_bridge/include/RosBridgeClient.h" -#include "ros_bridge/include/TypeFactory.h" -#include "ros_bridge/include/CasePacker.h" - -namespace ROSBridge { -namespace ComPrivate { - - -typedef std::function CallbackType; -struct MapStruct{ - typedef std::map Map; - Map map; - std::mutex mutex; -}; - -class TopicSubscriber -{ - typedef std::vector ClientList; -public: - - TopicSubscriber() = delete; - TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc); - ~TopicSubscriber(); - - //! @brief Starts the subscriber. - void start(); - - //! @brief Resets the subscriber. - void reset(); - - //! @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); - -private: - - - - CasePacker &_casePacker; - RosbridgeWsClient &_rbc; - MapStruct _callbackMapStruct; - ClientList _clientList; - bool _running; -}; - -void subscriberCallback(const HashType &hash, - MapStruct &mapWrapper, - std::shared_ptr /*connection*/, - std::shared_ptr in_message); -} // namespace ComPrivate -} // namespace ROSBridge diff --git a/src/comm/ros_bridge/include/TypeFactory.h b/src/comm/ros_bridge/include/TypeFactory.h deleted file mode 100644 index 44f4738278f67f7fb56bd72c4c0e043c5ee66b9b..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/include/TypeFactory.h +++ /dev/null @@ -1,137 +0,0 @@ -#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 - bool create(const rapidjson::Document &doc, T &type){ - static_assert(boost::is_base_of::value, "Type of msg must be derived from MessageBaseClass."); - static_assert(!::boost::is_same::value, - "msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)"); - return _create(doc, type, Type2Type()); - } - -private: - - // =========================================================================== - // EmptyGroup and not implemented Groups - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - (void)type; - (void)doc; - assert(false); // Implementation missing for group U::Group! - return false; - } - - // =========================================================================== - // Point32Group - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - using namespace ROSBridge; - bool ret = JsonMethodes::GeometryMsgs::Point32::fromJson(doc, type); - assert(ret); - return ret; - } - - // =========================================================================== - // GeoPointGroup - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - using namespace ROSBridge; - bool ret = JsonMethodes::GeographicMsgs::GeoPoint::fromJson(doc, type); - assert(ret); - return ret; - } - - // =========================================================================== - // PolygonGroup - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - using namespace ROSBridge; - bool ret = JsonMethodes::GeometryMsgs::Polygon::fromJson(doc, type); - assert(ret); - return ret; - } - - // =========================================================================== - // PolygonStampedGroup - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - using namespace ROSBridge; - bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::fromJson(doc, type); - assert(ret); - return ret; - } - - // =========================================================================== - // PolygonArrayGroup - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - using namespace ROSBridge; - bool ret = JsonMethodes::JSKRecognitionMsgs::PolygonArray::fromJson(doc, type); - assert(ret); - return ret; - } - - // =========================================================================== - // ProgressGroup - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - using namespace ROSBridge; - bool ret = JsonMethodes::NemoMsgs::Progress::fromJson(doc, type); - assert(ret); - return ret; - } - - // =========================================================================== - // HeartbeatGroup - // =========================================================================== - template - bool _create(const rapidjson::Document &doc, U &type, Type2Type){ - using namespace ROSBridge; - bool ret = JsonMethodes::NemoMsgs::Heartbeat::fromJson(doc, type); - assert(ret); - return ret; - } - - -}; - -} // end namespace ros_bridge diff --git a/src/comm/ros_bridge/include/com_private.cpp b/src/comm/ros_bridge/include/com_private.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8f61791d5e037ba30d2ef968807800599d0f49e7 --- /dev/null +++ b/src/comm/ros_bridge/include/com_private.cpp @@ -0,0 +1,53 @@ +#include "ros_bridge/include/com_private.h" + +#include "ros_bridge/rapidjson/include/rapidjson/writer.h" + +#include +#include + +std::size_t ros_bridge::com_private::getHash(const std::string &str) +{ + std::hash 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; +} diff --git a/src/comm/ros_bridge/include/ComPrivateInclude.h b/src/comm/ros_bridge/include/com_private.h similarity index 68% rename from src/comm/ros_bridge/include/ComPrivateInclude.h rename to src/comm/ros_bridge/include/com_private.h index d6285509917704fb5434b70ab38b13c5d58abc82..c5dcffead3fe1562d2c7518de2e00768752b2fc8 100644 --- a/src/comm/ros_bridge/include/ComPrivateInclude.h +++ b/src/comm/ros_bridge/include/com_private.h @@ -1,16 +1,14 @@ #pragma once -#include "ros_bridge/include/MessageTag.h" #include "ros_bridge/rapidjson/include/rapidjson/document.h" #include #include #include -namespace ROSBridge { -namespace ComPrivate { +namespace ros_bridge { +namespace com_private { -typedef MessageTag Tag; typedef rapidjson::Document JsonDoc; typedef std::unique_ptr JsonDocUPtr; typedef std::deque JsonQueue; @@ -20,11 +18,17 @@ using ClientMap = std::unordered_map; static const char* _topicAdvertiserKey = "topic_advertiser"; static const char* _topicPublisherKey = "topic_publisher"; -//static const char* _topicAdvertiserKey = "service_advertiser"; +static const char* _serviceAdvertiserKey = "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); + } } diff --git a/src/comm/ros_bridge/include/MessageTraits.h b/src/comm/ros_bridge/include/message_traits.h similarity index 97% rename from src/comm/ros_bridge/include/MessageTraits.h rename to src/comm/ros_bridge/include/message_traits.h index 4c54c9819f64c4627884c6731760bd41b4077eb5..449380bad15817de5993b12d2294f5142107a45c 100644 --- a/src/comm/ros_bridge/include/MessageTraits.h +++ b/src/comm/ros_bridge/include/message_traits.h @@ -1,6 +1,6 @@ #pragma once -namespace ROSBridge { +namespace ros_bridge { 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 +struct Type2Type{ + typedef T OriginalType; +}; + + +template +struct Int2Type{ + enum {value = k}; +}; + } } diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp new file mode 100644 index 0000000000000000000000000000000000000000..061bea1f088a46bd8dd6df19ac530c817a3fe79e --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp @@ -0,0 +1,5 @@ +#include "geopoint.h" + +std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){ + return "geographic_msgs/GeoPoint"; +} diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h new file mode 100644 index 0000000000000000000000000000000000000000..6a5e90e2a8cd5c9c32a58b30431bde62b25909b0 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h @@ -0,0 +1,149 @@ +#pragma once + +#include + +#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 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 + auto getAltitude(const T &p, traits::Type2Type) + { + return p.altitude(); + } + + template + auto getAltitude(const T &p, traits::Type2Type) + { + (void)p; + return 0.0; + } + + template + void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type) + { + p.setAltitude(doc.GetFloat()); + } + + template + void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type) + { + (void)doc; + (void)p; + } +} // namespace detail + +template +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::value, + traits::Has3Components, + traits::Has2Components>::Result + Components; // Check if PointType has 2 or 3 dimensions. + auto altitude = detail::getAltitude(p, traits::Type2Type()); // If T has no member altitude() replace it by 0.0; + + value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator); + return true; +} + + + +template +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::value, + traits::Has3Components, + traits::Has2Components>::Result + Components; // Check if PointType has 2 or 3 dimensions. + detail::setAltitude(value["altitude"], p, traits::Type2Type()); // If T has no member altitude() discard doc["altitude"]; + + return true; +} // namespace detail + + +} // namespace geopoint +} // namespace geometry_msgs +} // namespace messages +} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2736636eda3d28c21bc0cb3fd84d8ca9c80ba69f --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp @@ -0,0 +1,6 @@ +#include "point32.h" + +std::string ros_bridge::messages::geometry_msgs::point32::messageType(){ + return "geometry_msgs/Point32"; +} + diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h new file mode 100644 index 0000000000000000000000000000000000000000..c6e6865fd979ca377f8220ea9af48d42fcd8f09c --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h @@ -0,0 +1,139 @@ +#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 +auto getZ(const T &p, Type2Type) +{ + return p.z(); +} + +template +auto getZ(const T &p, Type2Type) +{ + (void)p; + return 0.0; // p has no member z() -> add 0. +} + +template +bool setZ(const rapidjson::Value &doc, const T &p, Type2Type) +{ + p.setZ(doc["z"].GetFloat()); + return true; +} + +template +bool setZ(const rapidjson::Value &doc, const T &p, Type2Type) +{ + (void)doc; + (void)p; + return true; +} + +} // namespace detail + +//! @brief C++ representation of a geometry_msgs/Point/Point32 +template +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 +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::value, Has3Components, Has2Components>::Result + Components; // Check if PointType has 2 or 3 dimensions. + auto z = detail::getZ(p, Type2Type()); // If T has no member z() replace it by 0. + + value.AddMember("z", rapidjson::Value().SetFloat(z), allocator); + return true; +} + +template +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::value, Has3Components, Has2Components>::Result + Components; // Check if PointType has 2 or 3 dimensions. + (void)detail::setZ(value["z"], p, Type2Type()); // If PointType has no member z() discard doc["z"]. + + return true; +} + +} // namespace point32 +} // namespace geometry_msgs +} // namespace messages +} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9c67ca377e67afa3873ac57c431f6bea06cd0375 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp @@ -0,0 +1,6 @@ +#include "polygon.h" + +std::string ros_bridge::messages::geometry_msgs::polygon::messageType(){ + return "geometry_msgs/Polygon"; +} + diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h new file mode 100644 index 0000000000000000000000000000000000000000..e5d815c106bf7c66a5a8ca2c78e790afc41b6d48 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h @@ -0,0 +1,91 @@ +#pragma once +#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include "ros_bridge/include/messages/geometry_msgs/point32.h" + +#include +#include + +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 ContainerType = std::vector> +class GenericPolygon { + typedef typename std::remove_cv_t> PointType; +public: + GenericPolygon() {} + GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){} + + + const ContainerType &points() const {return _points;} + ContainerType &points() {return _points;} + + GenericPolygon &operator=(const GenericPolygon &other) { + this->_points = other._points; + return *this; + } + +private: + ContainerType _points; +}; +typedef GenericPolygon Polygon; + + +template +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 +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> 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 diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0e974bca76e367642fe9f2d26aa52d0263799cef --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp @@ -0,0 +1,6 @@ +#include "polygon_stamped.h" + +std::string ros_bridge::messages::geometry_msgs::polygon_stamped::messageType(){ + return "geometry_msgs/PolygonStamped"; +} + diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h new file mode 100644 index 0000000000000000000000000000000000000000..49dfbff5757d13ba60feb88b97d987b177c090c3 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h @@ -0,0 +1,172 @@ +#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 + +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 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 +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> HeaderType; + HeaderType header; + bool ret = header::fromJson(doc, header); + polyStamped.header() = header; + return ret; +} +template +bool setHeader(const rapidjson::Value &doc, + PolygonStampedType &polyStamped, + traits::Int2Type<0>) { // polyStamped.header() does not exists + (void)doc; + (void)polyStamped; + return true; +} + + +template +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 +bool _toJson(const PolyStamped &polyStamped, + rapidjson::Value &value, + rapidjson::Document::AllocatorType &allocator, + traits::Int2Type){ // PolyStamped has member header(), use integraded header. +return _toJson(polyStamped, polyStamped.header(), value, allocator); +} + +template +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 +bool toJson(const PolygonType &poly, + const HeaderType &h, + rapidjson::Value &value, + rapidjson::Document::AllocatorType &allocator) +{ + return detail::_toJson(poly, h, value, allocator); +} + +template +bool toJson(const PolyStamped &polyStamped, + rapidjson::Value &value, + rapidjson::Document::AllocatorType &allocator) +{ + typedef traits::HasMemberHeader HasHeader; + return detail::_toJson(polyStamped, value, allocator, traits::Int2Type()); +} + +template +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 HasHeader; + if ( !detail::setHeader(value["header"], polyStamped, traits::Int2Type())){ + 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 diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp new file mode 100644 index 0000000000000000000000000000000000000000..844dc874423d8080446692bc52dec3b953956cd4 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp @@ -0,0 +1,6 @@ +#include "polygon_array.h" + +std::string ros_bridge::messages::jsk_recognition_msgs::polygon_array::messageType(){ + return "jsk_recognition_msgs/PolygonArray"; +} + diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h new file mode 100644 index 0000000000000000000000000000000000000000..05a5802ee9ff9d24132d031d7f543c217bc19484 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h @@ -0,0 +1,328 @@ +#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 +#include + +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 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 &polygons, + const ContainerType &labels, + const ContainerType &likelihood) + : _header(header) + , _polygons(polygons) + , _labels(labels) + , _likelihood(likelihood) + {} + + const HeaderType &header() const {return _header;} + HeaderType &header() {return _header;} + const ContainerType &polygons() const {return _polygons;} + ContainerType &polygons() {return _polygons;} + const ContainerType &labels() const {return _labels;} + ContainerType &labels() {return _labels;} + const ContainerType &likelyhood() const {return _likelihood;} + ContainerType &likelyhood() {return _likelihood;} + + +private: + HeaderType _header; + ContainerType _polygons; + ContainerType _labels; + ContainerType _likelihood; +}; +typedef GenericPolygonArray<> PolygonArray; + + +namespace detail { + //! Helper functions to generate Json entries for labels and likelihood. + + //! \note \p p has member \fn labels(). + template + void labelsToJson(const PolygonArrayType &p, + rapidjson::Value &labels, + rapidjson::Document::AllocatorType &allocator, + traits::Int2Type){ + 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 + 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 + void likelihoodToJson(const PolygonArrayType &p, + rapidjson::Value &likelyhood, + rapidjson::Document::AllocatorType &allocator, + traits::Int2Type){ + 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 + 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 + void setLabels(const rapidjson::Value &doc, + PolygonArrayType &p, + traits::Int2Type){ + 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 + void setLabels(const rapidjson::Value &doc, + PolygonArrayType &p, + traits::Int2Type<0>){ + (void)doc; + (void)p; + } + + //! \note \p p has member \fn likelihood(). + template + void setLikelihood(const rapidjson::Value &doc, + PolygonArrayType &p, + traits::Int2Type){ + 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 + void setLikelihood(const rapidjson::Value &doc, + PolygonArrayType &p, + traits::Int2Type<0>){ + (void)doc; + (void)p; + } + + template + 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 HasLabels; + detail::labelsToJson(p, jLabels, allocator, traits::Int2Type()); + value.AddMember("labels", jLabels, allocator); + + + rapidjson::Value jLikelihood(rapidjson::kArrayType); + typedef traits::HasMemberLikelihood HasLikelihood; + detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type()); + value.AddMember("likelihood", jLikelihood, allocator); + + return true; + } + + template + bool _toJson(const PolygonArrayType &p, + rapidjson::Value &value, + rapidjson::Document::AllocatorType &allocator, + traits::Int2Type) + { + // U has member header(), use integraded header. + return _toJson(p, p.header(), value, allocator); + } + + template + 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 +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 +bool toJson(const PolygonArrayType &p, + rapidjson::Value &value, + rapidjson::Document::AllocatorType &allocator) +{ + typedef traits::HasMemberHeader HasHeader; + return detail::_toJson(p, value, allocator, traits::Int2Type()); +} + + + +template +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 HasHeader; + if ( !polygon_stamped::detail::setHeader(value["header"], + p, + traits::Int2Type())){ + 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> + 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())){ + 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 HasLabels; + detail::setLabels(value["labels"], p, traits::Int2Type()); + + typedef traits::HasMemberLikelihood HasLikelihood; + detail::setLikelihood(value["likelihood"], p, traits::Int2Type()); + + return true; +} + + +} // namespace polygon_array +} // namespace geometry_msgs +} // namespace messages +} // namespace ros_bridge diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8ba039910c45c4c5f72e511972d130a58dc3c0fc --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp @@ -0,0 +1,6 @@ +#include "heartbeat.h" + +std::string ros_bridge::messages::nemo_msgs::heartbeat::messageType(){ + return "nemo_msgs/Heartbeat"; +} + diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h new file mode 100644 index 0000000000000000000000000000000000000000..6f53c59af9bc4b038205410d30e93795cdd28630 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h @@ -0,0 +1,53 @@ +#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 +bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) +{ + value.AddMember("status", std::int32_t(heartbeat.status()), allocator); + return true; +} + +template +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 diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0b408a2df384d229802f0e53cebdab9dcac2677f --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp @@ -0,0 +1,6 @@ +#include "progress.h" + +std::string ros_bridge::messages::nemo_msgs::progress::messageType(){ + return "nemo_msgs/Progress"; +} + diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h new file mode 100644 index 0000000000000000000000000000000000000000..12b3bbe06d1985e92ed900364b2e58fc1bf966ec --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h @@ -0,0 +1,67 @@ +#pragma once + +#include "ros_bridge/rapidjson/include/rapidjson/document.h" + +#include + + +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 ContainterType = std::vector> +class GenericProgress{ +public: + GenericProgress() {} + GenericProgress(const ContainterType &progress) : _progress(progress){} + GenericProgress(const GenericProgress &p) : _progress(p.progress()){} + + virtual const ContainterType &progress(void) const {return _progress;} + virtual ContainterType &progress(void) {return _progress;} + +protected: + ContainterType _progress; +}; +typedef GenericProgress<> Progress; + + +template +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 +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 diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.cpp b/src/comm/ros_bridge/include/messages/std_msgs/header.cpp new file mode 100644 index 0000000000000000000000000000000000000000..bd9cc9467423552d7af05778ce22d5899af93a4f --- /dev/null +++ b/src/comm/ros_bridge/include/messages/std_msgs/header.cpp @@ -0,0 +1,69 @@ +#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"; +} diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.h b/src/comm/ros_bridge/include/messages/std_msgs/header.h new file mode 100644 index 0000000000000000000000000000000000000000..e3a6546a020c3fd793d9bcc64ac53d4714e275db --- /dev/null +++ b/src/comm/ros_bridge/include/messages/std_msgs/header.h @@ -0,0 +1,102 @@ +#pragma once + +#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include "ros_bridge/include/messages/std_msgs/time.h" + +#include + +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 +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 +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 diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.cpp b/src/comm/ros_bridge/include/messages/std_msgs/time.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7d41fe8ffb5391125401e540497fe4b645f90ee8 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/std_msgs/time.cpp @@ -0,0 +1,5 @@ +#include "time.h" + +std::string ros_bridge::messages::std_msgs::time::messageType(){ + return "std_msgs/Time"; +} diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.h b/src/comm/ros_bridge/include/messages/std_msgs/time.h new file mode 100644 index 0000000000000000000000000000000000000000..c3f7216141903e0a4d98a8a97a0c1866cfe654ef --- /dev/null +++ b/src/comm/ros_bridge/include/messages/std_msgs/time.h @@ -0,0 +1,65 @@ +#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 +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 +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 diff --git a/src/comm/ros_bridge/include/ros_bridge.h b/src/comm/ros_bridge/include/ros_bridge.h new file mode 100644 index 0000000000000000000000000000000000000000..86e71f099d161c46fb6ba6d389d06a46761fc673 --- /dev/null +++ b/src/comm/ros_bridge/include/ros_bridge.h @@ -0,0 +1,49 @@ +#pragma once + +#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include "ros_bridge/include/topic_publisher.h" +#include "ros_bridge/include/topic_subscriber.h" +#include "ros_bridge/include/server.h" + +#include +#include + +namespace ros_bridge { +class ROSBridge +{ +public: + typedef rapidjson::Document JsonDoc; + typedef std::unique_ptr JsonDocUPtr; + + explicit ROSBridge(); + explicit ROSBridge(const char* connectionString); + void publish(JsonDocUPtr doc, const char* topic); + void subscribe(const char *topic, + const std::function &callBack); + void advertiseService(const char* service, + const char* type, + const std::function &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 \fn calls start(). + bool connected(); + bool isRunning(); + +private: + std::shared_ptr _stopped; + RosbridgeWsClient _rbc; + com_private::TopicPublisher _topicPublisher; + com_private::TopicSubscriber _topicSubscriber; + com_private::Server _server; + +}; + +bool isValidConnectionString(const char* connectionString); +} diff --git a/src/comm/ros_bridge/include/server.cpp b/src/comm/ros_bridge/include/server.cpp new file mode 100644 index 0000000000000000000000000000000000000000..eca731e71acb3366cb1a9e1cd786317239d32a15 --- /dev/null +++ b/src/comm/ros_bridge/include/server.cpp @@ -0,0 +1,99 @@ +#include "server.h" + +#include "rapidjson/include/rapidjson/ostreamwrapper.h" + +ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) : + _rbc(rbc) + , _stopped(std::make_shared(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, + std::shared_ptr 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(); +} diff --git a/src/comm/ros_bridge/include/server.h b/src/comm/ros_bridge/include/server.h new file mode 100644 index 0000000000000000000000000000000000000000..f1500b9ed7de552bcc3e7a686189c70afe5d026e --- /dev/null +++ b/src/comm/ros_bridge/include/server.h @@ -0,0 +1,39 @@ +#pragma once + +#include "ros_bridge/include/com_private.h" +#include "ros_bridge/include/RosBridgeClient.h" + +#include + +namespace ros_bridge { +namespace com_private { + +class Server +{ + typedef std::unordered_map ServiceMap; +public: + typedef std::function 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 _stopped; +}; +} // namespace ComPrivate +} // namespace ROSBridge diff --git a/src/comm/ros_bridge/include/topic_publisher.cpp b/src/comm/ros_bridge/include/topic_publisher.cpp new file mode 100644 index 0000000000000000000000000000000000000000..dbee9d683c8b34f3edc599296991bf6c417ea23b --- /dev/null +++ b/src/comm/ros_bridge/include/topic_publisher.cpp @@ -0,0 +1,147 @@ +#include "topic_publisher.h" + +#include + +ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc) : + _stopped(std::make_shared(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([this]{ + // Main Loop. + while( !this->_stopped->load() ){ + std::unique_lock 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 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 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 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 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; + } +} + diff --git a/src/comm/ros_bridge/include/topic_publisher.h b/src/comm/ros_bridge/include/topic_publisher.h new file mode 100644 index 0000000000000000000000000000000000000000..cbb84fb972af8f12b6888dacf46f2e51c9d448c6 --- /dev/null +++ b/src/comm/ros_bridge/include/topic_publisher.h @@ -0,0 +1,49 @@ +#pragma once + +#include "ros_bridge/include/com_private.h" +#include "ros_bridge/include/RosBridgeClient.h" + +#include +#include +#include +#include + +namespace ros_bridge { +namespace com_private { + +struct ThreadData; + +class TopicPublisher +{ + typedef std::unique_ptr ThreadPtr; + using CondVar = std::condition_variable; +public: + + TopicPublisher() = delete; + TopicPublisher(RosbridgeWsClient &rbc); + + ~TopicPublisher(); + + //! @brief Starts the publisher. + void start(); + + //! @brief Resets the publisher. + void reset(); + + void publish(JsonDocUPtr docPtr, const char *topic); + bool advertise(const char *topic, const char *type); + +private: + using TopicMap = std::unordered_map; + JsonQueue _queue; + TopicMap _topicMap; + std::mutex _mutex; + std::shared_ptr _stopped; + RosbridgeWsClient &_rbc; + CondVar _cv; + ThreadPtr _pThread; +}; + + +} // namespace CommunicatorDetail +} // namespace ROSBridge diff --git a/src/comm/ros_bridge/include/topic_subscriber.cpp b/src/comm/ros_bridge/include/topic_subscriber.cpp new file mode 100644 index 0000000000000000000000000000000000000000..966303bbfe48eb41838943463dbd12f459f988a7 --- /dev/null +++ b/src/comm/ros_bridge/include/topic_subscriber.cpp @@ -0,0 +1,99 @@ +#include "topic_subscriber.h" + +#include + +ros_bridge::com_private::TopicSubscriber::TopicSubscriber(RosbridgeWsClient &rbc) : + _rbc(rbc) + , _stopped(std::make_shared(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 &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, + std::shared_ptr 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; + } +} + + diff --git a/src/comm/ros_bridge/include/topic_subscriber.h b/src/comm/ros_bridge/include/topic_subscriber.h new file mode 100644 index 0000000000000000000000000000000000000000..eff43217dd67110726ee80c20bb9a96121950335 --- /dev/null +++ b/src/comm/ros_bridge/include/topic_subscriber.h @@ -0,0 +1,41 @@ +#pragma once + +#include "ros_bridge/include/com_private.h" +#include "ros_bridge/include/RosBridgeClient.h" + +#include + +namespace ros_bridge { +namespace com_private { + + +typedef std::function CallbackType; + +class TopicSubscriber +{ + typedef std::unordered_map TopicMap; +public: + + TopicSubscriber() = delete; + TopicSubscriber(RosbridgeWsClient &rbc); + ~TopicSubscriber(); + + //! @brief Starts the subscriber. + void start(); + + //! @brief Resets the subscriber. + void reset(); + + //! @return Returns false if a subscription to this topic allready exists. + //! + //! @note Only one callback can be registered. + void subscribe(const char* topic, const CallbackType &callback); + +private: + RosbridgeWsClient &_rbc; + TopicMap _topicMap; + std::shared_ptr _stopped; +}; + +} // namespace ComPrivate +} // namespace ROSBridge diff --git a/src/comm/ros_bridge/src/CasePacker.cpp b/src/comm/ros_bridge/src/CasePacker.cpp deleted file mode 100644 index cd17974f34e579eb8d0c3c4cec6ebe64c6ecced7..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/src/CasePacker.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#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; -} diff --git a/src/comm/ros_bridge/src/PackageBuffer.h b/src/comm/ros_bridge/src/PackageBuffer.h deleted file mode 100644 index c41163e23d5c7b18844945c9ccae2fea467da5d5..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/src/PackageBuffer.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include "boost/lockfree/queue.hpp" -#include - -namespace ROSBridge { -namespace Bridge { -namespace lf = ::boost::lockfree; -template -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 pushCallback) { - _pushCallback = pushCallback; - } - - void setPopCallback(std::function popCallback) { - _popCallback = popCallback; - } - - bool empty() const { return buffer.empty();} -private: - lf::queue buffer; - std::function _popCallback; - std::function _pushCallback; -}; - -} // namespace Communicator - -} // namespace diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp deleted file mode 100644 index 32d8ab791301c465a0bde1fcde3da25b02af33b6..0000000000000000000000000000000000000000 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#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 &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(); -} - diff --git a/src/comm/ros_bridge/src/ros_bridge.cpp b/src/comm/ros_bridge/src/ros_bridge.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0e98af2974203d15d80cc92177e4b4ac511f78e1 --- /dev/null +++ b/src/comm/ros_bridge/src/ros_bridge.cpp @@ -0,0 +1,81 @@ +#include "ros_bridge/include/ros_bridge.h" + + +ros_bridge::ROSBridge::ROSBridge(const char *connectionString) : + _stopped(std::make_shared(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 &callBack) +{ + _topicSubscriber.subscribe(topic, callBack); +} + +void ros_bridge::ROSBridge::advertiseService(const char *service, + const char *type, + const std::function &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(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); +} diff --git a/src/comm/utilities.h b/src/comm/utilities.h index a3556846a01f1e823a8fd8041ccbfbc1d6254076..96a4c74c994b33e5b4773dee0ce09d4844779e1a 100644 --- a/src/comm/utilities.h +++ b/src/comm/utilities.h @@ -73,15 +73,4 @@ private: bool _isInitialized; }; -template -struct Type2Type{ - typedef T OriginalType; -}; - - -template -struct Int2Type{ - enum {value = k}; -}; - #endif // UTILITIES_H diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index e896225cfd84ef856cd03484ef980168fc529856..a0efe586c45f063e20d7f0f197203fef401e00e2 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -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 } } }