From 95f56761b6c9888afb80116e2c2df78da66bd900 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Fri, 21 Aug 2020 15:15:49 +0200 Subject: [PATCH] json stuff improved --- qgroundcontrol.pro | 39 +- src/Wima/Geometry/GeoPoint3D.h | 9 +- src/Wima/Geometry/Polygon2D.h | 8 +- src/Wima/Geometry/PolygonArray.h | 2 +- src/Wima/Geometry/WimaPolygonArray.h | 17 +- src/Wima/Snake/QNemoHeartbeat.h | 4 +- src/Wima/Snake/QNemoProgress.h | 10 +- src/Wima/Snake/QtROSJsonFactory.h | 5 - src/Wima/Snake/QtROSTypeFactory.h | 5 - src/Wima/Snake/SnakeTilesLocal.h | 3 +- src/Wima/WimaController.cc | 94 ++- src/Wima/WimaController.h | 7 +- src/comm/ros_bridge/include/CasePacker.h | 54 -- .../ros_bridge/include/ComPrivateInclude.cpp | 13 - src/comm/ros_bridge/include/GenericMessages.h | 371 ---------- src/comm/ros_bridge/include/JsonFactory.h | 238 ------ src/comm/ros_bridge/include/JsonMethodes.h | 695 ------------------ .../ros_bridge/include/MessageBaseClass.h | 30 - src/comm/ros_bridge/include/MessageGroups.h | 158 ---- src/comm/ros_bridge/include/MessageTag.cpp | 43 -- src/comm/ros_bridge/include/MessageTag.h | 24 - src/comm/ros_bridge/include/ThreadSafeQueue.h | 79 -- .../ros_bridge/include/TopicPublisher.cpp | 103 --- src/comm/ros_bridge/include/TypeFactory.h | 137 ---- src/comm/ros_bridge/include/com_private.cpp | 49 ++ .../{ComPrivateInclude.h => com_private.h} | 10 +- .../{MessageTraits.h => message_traits.h} | 16 +- .../messages/geographic_msgs/geopoint.h | 152 ++++ .../include/messages/geometry_msgs/point32.h | 142 ++++ .../include/messages/geometry_msgs/polygon.h | 95 +++ .../messages/geometry_msgs/polygon_stamped.h | 175 +++++ .../jsk_recognition_msgs/polygon_array.h | 332 +++++++++ .../include/messages/nemo_msgs/heartbeat.h | 56 ++ .../include/messages/nemo_msgs/progress.h | 70 ++ .../include/messages/std_msgs/header.h | 106 +++ .../include/messages/std_msgs/time.h | 68 ++ .../include/{ROSBridge.h => ros_bridge.h} | 38 +- .../include/{Server.cpp => server.cpp} | 12 +- .../ros_bridge/include/{Server.h => server.h} | 6 +- .../ros_bridge/include/topic_publisher.cpp | 105 +++ .../{TopicPublisher.h => topic_publisher.h} | 25 +- ...picSubscriber.cpp => topic_subscriber.cpp} | 23 +- .../{TopicSubscriber.h => topic_subscriber.h} | 16 +- src/comm/ros_bridge/src/CasePacker.cpp | 69 -- src/comm/ros_bridge/src/ROSBridge.cpp | 76 -- src/comm/ros_bridge/src/ros_bridge.cpp | 71 ++ src/comm/utilities.h | 11 - 47 files changed, 1581 insertions(+), 2290 deletions(-) delete mode 100644 src/Wima/Snake/QtROSJsonFactory.h delete mode 100644 src/Wima/Snake/QtROSTypeFactory.h delete mode 100644 src/comm/ros_bridge/include/CasePacker.h delete mode 100644 src/comm/ros_bridge/include/ComPrivateInclude.cpp delete mode 100644 src/comm/ros_bridge/include/GenericMessages.h delete mode 100644 src/comm/ros_bridge/include/JsonFactory.h delete mode 100644 src/comm/ros_bridge/include/JsonMethodes.h delete mode 100644 src/comm/ros_bridge/include/MessageBaseClass.h delete mode 100644 src/comm/ros_bridge/include/MessageGroups.h delete mode 100644 src/comm/ros_bridge/include/MessageTag.cpp delete mode 100644 src/comm/ros_bridge/include/MessageTag.h delete mode 100644 src/comm/ros_bridge/include/ThreadSafeQueue.h delete mode 100644 src/comm/ros_bridge/include/TopicPublisher.cpp delete mode 100644 src/comm/ros_bridge/include/TypeFactory.h create mode 100644 src/comm/ros_bridge/include/com_private.cpp rename src/comm/ros_bridge/include/{ComPrivateInclude.h => com_private.h} (77%) rename src/comm/ros_bridge/include/{MessageTraits.h => message_traits.h} (97%) create mode 100644 src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h create mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/point32.h create mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h create mode 100644 src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h create mode 100644 src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/progress.h create mode 100644 src/comm/ros_bridge/include/messages/std_msgs/header.h create mode 100644 src/comm/ros_bridge/include/messages/std_msgs/time.h rename src/comm/ros_bridge/include/{ROSBridge.h => ros_bridge.h} (54%) rename src/comm/ros_bridge/include/{Server.cpp => server.cpp} (91%) rename src/comm/ros_bridge/include/{Server.h => server.h} (90%) create mode 100644 src/comm/ros_bridge/include/topic_publisher.cpp rename src/comm/ros_bridge/include/{TopicPublisher.h => topic_publisher.h} (56%) rename src/comm/ros_bridge/include/{TopicSubscriber.cpp => topic_subscriber.cpp} (76%) rename src/comm/ros_bridge/include/{TopicSubscriber.h => topic_subscriber.h} (70%) delete mode 100644 src/comm/ros_bridge/src/CasePacker.cpp delete mode 100644 src/comm/ros_bridge/src/ROSBridge.cpp create mode 100644 src/comm/ros_bridge/src/ros_bridge.cpp diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3d4c5dfd6..f8045a1b8 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -437,8 +437,7 @@ HEADERS += \ src/Wima/Geometry/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 \ @@ -483,17 +482,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/Server.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/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/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 \ @@ -511,13 +514,11 @@ 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/RosBridgeClient.cpp \ - src/comm/ros_bridge/include/Server.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/com_private.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 \ @@ -549,7 +550,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/Wima/Geometry/GeoPoint3D.h b/src/Wima/Geometry/GeoPoint3D.h index 407297003..5f61a9f83 100644 --- a/src/Wima/Geometry/GeoPoint3D.h +++ b/src/Wima/Geometry/GeoPoint3D.h @@ -1,16 +1,11 @@ #pragma once -#include "ros_bridge/include/JsonMethodes.h" -#include "ros_bridge/include/MessageBaseClass.h" -#include "ros_bridge/include/GenericMessages.h" - #include #include -typedef ROSBridge::MessageBaseClass ROSMsg; -typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint; -namespace MsgGroups = ROSBridge::MessageGroups; +typedef ros_bridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint; +namespace MsgGroups = ros_bridge::MessageGroups; class GeoPoint3D : public QObject, public ROSGeoPoint { Q_OBJECT diff --git a/src/Wima/Geometry/Polygon2D.h b/src/Wima/Geometry/Polygon2D.h index 4ced5c058..7c4595e9b 100644 --- a/src/Wima/Geometry/Polygon2D.h +++ b/src/Wima/Geometry/Polygon2D.h @@ -8,14 +8,14 @@ #include "ros_bridge/include/MessageBaseClass.h" #include "ros_bridge/include/JsonMethodes.h" -namespace MsgGroupsNS = ROSBridge::MessageGroups; -namespace PolyStampedNS = ROSBridge::JsonMethodes::GeometryMsgs::PolygonStamped; +namespace MsgGroupsNS = ros_bridge::MessageGroups; +namespace PolyStampedNS = ros_bridge::JsonMethodes::GeometryMsgs::PolygonStamped; -typedef ROSBridge::MessageBaseClass ROSMsg; +typedef ros_bridge::MessageBaseClass ROSMsg; template class ContainerType = QVector> class Polygon2DTemplate : public ROSMsg{ // - typedef ROSBridge::GenericMessages::GeometryMsgs::GenericPolygon Poly; + typedef ros_bridge::GenericMessages::GeometryMsgs::GenericPolygon Poly; public: typedef MsgGroupsNS::PolygonStampedGroup Group; // has no header diff --git a/src/Wima/Geometry/PolygonArray.h b/src/Wima/Geometry/PolygonArray.h index 9a658169f..bd933604b 100644 --- a/src/Wima/Geometry/PolygonArray.h +++ b/src/Wima/Geometry/PolygonArray.h @@ -4,7 +4,7 @@ #include "ros_bridge/include/MessageBaseClass.h" -typedef ROSBridge::MessageBaseClass ROSMsgBase; +typedef ros_bridge::MessageBaseClass ROSMsgBase; template class ContainerType > class PolygonArray : public ROSMsgBase, public ContainerType { public: diff --git a/src/Wima/Geometry/WimaPolygonArray.h b/src/Wima/Geometry/WimaPolygonArray.h index f43f4ca15..c489ab15f 100644 --- a/src/Wima/Geometry/WimaPolygonArray.h +++ b/src/Wima/Geometry/WimaPolygonArray.h @@ -1,30 +1,19 @@ #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); - } - class QmlObjectListModel *QmlObjectListModel(){ if (_dirty) _updateObjects(); diff --git a/src/Wima/Snake/QNemoHeartbeat.h b/src/Wima/Snake/QNemoHeartbeat.h index 840ab74e3..8e95986eb 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 1f2d5e037..0d838984b 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_msgs = ros_bridge::messages::nemo_msgs; +typedef nemo_msgs::progress::GenericProgress QNemoProgress; diff --git a/src/Wima/Snake/QtROSJsonFactory.h b/src/Wima/Snake/QtROSJsonFactory.h deleted file mode 100644 index a58386e3f..000000000 --- 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 a2e43383d..000000000 --- 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/Snake/SnakeTilesLocal.h b/src/Wima/Snake/SnakeTilesLocal.h index 76f8b3702..1a81aadc7 100644 --- a/src/Wima/Snake/SnakeTilesLocal.h +++ b/src/Wima/Snake/SnakeTilesLocal.h @@ -4,5 +4,4 @@ #include "Wima/Geometry/Polygon2D.h" #include "Wima/Geometry/WimaPolygonArray.h" -namespace MsgGroups = ROSBridge::MessageGroups; -typedef WimaPolygonArray SnakeTilesLocal; +typedef WimaPolygonArray SnakeTilesLocal; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index b0424af3c..fb4040ef9 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -1,10 +1,14 @@ #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" @@ -94,11 +98,11 @@ WimaController::WimaController(QObject *parent) auto connectionStringFact = wimaSettings->rosbridgeConnectionString(); auto setConnectionString = [connectionStringFact, this]{ auto connectionString = connectionStringFact->rawValue().toString(); - if ( ROSBridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){ - this->_pRosBridge.reset(new ROSBridge::ROSBridge(connectionString.toLocal8Bit().data())); + 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 ROSBridge::ROSBridge("localhost:9090")); + this->_pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090")); } }; setConnectionString(); @@ -939,22 +943,40 @@ void WimaController::_switchSnakeManager(QVariant variant) void WimaController::_setupTopicService() { + using namespace ros_bridge::messages; if ( _snakeTilesLocal.polygons().size() > 0){ - _pRosBridge->publish(_snakeOrigin, "/snake/origin"); - _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); + // Publish snake origin. + JsonDocUPtr jOrigin(rapidjson::kObjectType); + Q_ASSERT(geographic_msgs::geo_point::toJson( + _snakeOrigin, *jOrigin, jOrigin->GetAllocator())); + _pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + // Publish snake tiles. + JsonDocUPtr jSnakeTiles(rapidjson::kObjectType); + Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson( + _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator())); + _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 = this->_nemoProgress; - if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress) - || progress.progress().size() != requiredSize ) { // Some error occured. + 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.progress().fill(0, requiredSize); - // Publish origin and tiles again. + progress_msg.progress().fill(0, requiredSize); + // Publish origin and tiles again, if valid. if ( this->_snakeTilesLocal.polygons().size() > 0){ - this->_pRosBridge->publish(this->_snakeOrigin, "/snake/origin"); - this->_pRosBridge->publish(this->_snakeTilesLocal, "/snake/tiles"); + // Publish snake origin. + JsonDocUPtr jOrigin(rapidjson::kObjectType); + Q_ASSERT(geographic_msgs::geo_point::toJson( + this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator())); + this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + // Publish snake tiles. + JsonDocUPtr jSnakeTiles(rapidjson::kObjectType); + Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson( + this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator())); + this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); } } @@ -962,10 +984,11 @@ void WimaController::_setupTopicService() }); _pRosBridge->subscribe("/nemo/heartbeat", /* callback */ [this](JsonDocUPtr pDoc){ - if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) { - if ( this->_nemoHeartbeat.status() == this->_fallbackStatus ) + auto &heartbeat_msg = this->_nemoHeartbeat; + if ( !nemo_msgs::heartbeat(*pDoc, heartbeat_msg) ) { + if ( heartbeat_msg.status() == this->_fallbackStatus ) return; - this->_nemoHeartbeat.setStatus(this->_fallbackStatus); + heartbeat_msg.setStatus(this->_fallbackStatus); } this->_nemoTimeoutTicker.reset(); @@ -975,30 +998,29 @@ void WimaController::_setupTopicService() }); _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin", - [this](JsonDocUPtr) -> JsonDocUPtr { - JsonDocUPtr pDoc; + [this](JsonDocUPtr) -> JsonDocUPtr + { + JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); + rapidjson::Value jOrigin(rapidjson::kObjectType); if ( this->_snakeTilesLocal.polygons().size() > 0){ - pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeOrigin, ""); + geometry_msg::geo_point::toJson( + this->_snakeOrigin, jOrigin, pDoc->GetAllocator()); } else { - pDoc = this->_pRosBridge->casePacker()->pack(::GeoPoint3D(0,0,0), ""); + geometry_msg::geo_point::polygon_array::toJson( + ::GeoPoint3D(0,0,0), jOrigin, pDoc->GetAllocator()); } - this->_pRosBridge->casePacker()->removeTag(pDoc); - rapidjson::Document value(rapidjson::kObjectType); - JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType)); - value.CopyFrom(*pDoc, pReturn->GetAllocator()); - pReturn->AddMember("origin", value, pReturn->GetAllocator()); - - return pReturn; + pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); + return pDoc; }); _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles", - [this](JsonDocUPtr) -> JsonDocUPtr { - JsonDocUPtr pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeTilesLocal, ""); - this->_pRosBridge->casePacker()->removeTag(pDoc); - rapidjson::Document value(rapidjson::kObjectType); - JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType)); - value.CopyFrom(*pDoc, pReturn->GetAllocator()); - pReturn->AddMember("tiles", value, pReturn->GetAllocator()); - return pReturn; + [this](JsonDocUPtr) -> JsonDocUPtr + { + JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); + rapidjson::Value jSnakeTiles(rapidjson::kObjectType); + jsk_recognition_msgs::polygon_array::toJson( + this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator()); + pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); + return pDoc; }); } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 7996f361c..8535a4fe0 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -35,8 +35,7 @@ #include "Snake/QNemoProgress.h" #include "Snake/QNemoHeartbeat.h" -#include "ros_bridge/include/ROSBridge.h" -#include "ros_bridge/include/CasePacker.h" +#include "ros_bridge/include/ros_bridge.h" #include "WaypointManager/DefaultManager.h" #include "WaypointManager/RTLManager.h" @@ -54,7 +53,7 @@ class WimaController : public QObject enum FileType {WimaFile, PlanFile}; - typedef QScopedPointer ROSBridgePtr; + typedef QScopedPointer ROSBridgePtr; public: @@ -344,7 +343,7 @@ private slots: private: using StatusMap = std::map; - using CasePacker = ROSBridge::CasePacker; + using CasePacker = ros_bridge::CasePacker; // Controllers. PlanMasterController *_masterController; diff --git a/src/comm/ros_bridge/include/CasePacker.h b/src/comm/ros_bridge/include/CasePacker.h deleted file mode 100644 index 3bf889a74..000000000 --- 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 ac583a76d..000000000 --- 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 8acb15a33..000000000 --- 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() : MessageBaseClass() {} - GenericProgress(const ContainterType &progress) : MessageBaseClass(), _progress(progress){} - GenericProgress(const GenericProgress &p) :MessageBaseClass(), _progress(p.progress()){} - ~GenericProgress() {} - - virtual GenericProgress *Clone() const override { return new GenericProgress(*this); } - - virtual const ContainterType &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 e3e1d6237..000000000 --- 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 9444a4c33..000000000 --- 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 b2d032c73..000000000 --- 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 7b95eef76..000000000 --- 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 80a74d9c9..000000000 --- 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 cc04dc6b8..000000000 --- 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/ThreadSafeQueue.h b/src/comm/ros_bridge/include/ThreadSafeQueue.h deleted file mode 100644 index 03d653eee..000000000 --- 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 2f78a3cf2..000000000 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include "TopicPublisher.h" - -#include - -ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker, - RosbridgeWsClient &rbc) : - _stopped(std::make_shared(true)) - , _casePacker(casePacker) - , _rbc(rbc) -{ - -} - -ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher() -{ - this->reset(); -} - -void ROSBridge::ComPrivate::TopicPublisher::start() -{ - if ( !_stopped->load() ) // start called while thread running. - return; - _stopped->store(false); - _pThread = std::make_unique([this]{ - // Init. - std::unordered_map topicMap; - // Main Loop. - while( !this->_stopped->load() ){ - JsonDocUPtr pJsonDoc; - { - 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. - pJsonDoc = std::move(this->_queue.front()); - this->_queue.pop_front(); - } - - // Get tag from Json message and remove it. - Tag tag; - bool ret = this->_casePacker.getTag(pJsonDoc, tag); - assert(ret); // Json message does not contain a tag; - (void)ret; - this->_casePacker.removeTag(pJsonDoc); - - // Check if topic must be advertised. - // Advertised topics are stored in advertisedTopicsHashList as - // a hash. - std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey - + tag.topic(); - auto it = topicMap.find(clientName); - if ( it == topicMap.end()) { // Need to advertise topic? - topicMap.insert(std::make_pair(clientName, tag.topic())); - this->_rbc.addClient(clientName); - this->_rbc.advertise(clientName, - tag.topic(), - tag.messageType() ); - this->_rbc.waitForTopic(tag.topic(), [this]{ - return this->_stopped->load(); - }); // Wait until topic is advertised. - } - - // Publish Json message. - if ( !this->_stopped->load() ) - this->_rbc.publish(tag.topic(), *pJsonDoc.get()); - } // while loop - - // Tidy up. - for (auto& it : topicMap){ - this->_rbc.unadvertise(it.second); - this->_rbc.removeClient(it.first); - } - - std::cout << "TopicPublisher: publisher thread terminated." << std::endl; - }); -} - -void ROSBridge::ComPrivate::TopicPublisher::reset() -{ - if ( _stopped->load() ) // stop called while thread not running. - return; - { - std::lock_guard lk(_mutex); - std::cout << "TopicPublisher: _stopped->store(true)." << std::endl; - _stopped->store(true); - std::cout << "TopicPublisher: ask publisher thread to stop." << std::endl; - _cv.notify_one(); // Wake publisher thread. - } - if ( !_pThread ) - return; - _pThread->join(); - std::cout << "TopicPublisher: publisher thread joined." << std::endl; - { - _queue.clear(); - std::cout << "TopicPublisher: queue cleard." << std::endl; - } -} - diff --git a/src/comm/ros_bridge/include/TypeFactory.h b/src/comm/ros_bridge/include/TypeFactory.h deleted file mode 100644 index 44f473827..000000000 --- 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 000000000..001be47e9 --- /dev/null +++ b/src/comm/ros_bridge/include/com_private.cpp @@ -0,0 +1,49 @@ +#include "ros_bridge/include/com_private.h" +#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 77% rename from src/comm/ros_bridge/include/ComPrivateInclude.h rename to src/comm/ros_bridge/include/com_private.h index ba6e137f6..ada0ab662 100644 --- a/src/comm/ros_bridge/include/ComPrivateInclude.h +++ b/src/comm/ros_bridge/include/com_private.h @@ -7,8 +7,8 @@ #include #include -namespace ROSBridge { -namespace ComPrivate { +namespace ros_bridge { +namespace com_private { typedef MessageTag Tag; typedef rapidjson::Document JsonDoc; @@ -26,5 +26,11 @@ 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 4c54c9819..449380bad 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.h b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h new file mode 100644 index 000000000..799687c4e --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h @@ -0,0 +1,152 @@ +#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(){ + return "geometry_msgs/GeoPoint"; +} + +//! @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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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.h b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h new file mode 100644 index 000000000..6788a43d9 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h @@ -0,0 +1,142 @@ +#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(){ + return "geometry_msgs/Point32"; +} + +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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h new file mode 100644 index 000000000..75b2f97df --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h @@ -0,0 +1,95 @@ +#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(){ + return "geometry_msgs/Polygon"; +} + +//! @brief C++ representation of geometry_msgs/Polygon +template class ContainerType = std::vector> +class GenericPolygon { + typedef typename std::remove_cv_ref_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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h new file mode 100644 index 000000000..da145c560 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h @@ -0,0 +1,175 @@ +#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(){ + return "geometry_msgs/PolygonStamped"; +} + +//! @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::header; + + 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::header; + using namespace geometry_msgs::polygon; + + 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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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.h b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h new file mode 100644 index 000000000..7da61863d --- /dev/null +++ b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h @@ -0,0 +1,332 @@ +#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(){ + return "jsk_recognition_msgs/PolygonArray"; +} + + +//! @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::header; + using namespace geometry_msgs::polygon_stamped; + + 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); + + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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, 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.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h new file mode 100644 index 000000000..50434997d --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h @@ -0,0 +1,56 @@ +#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(){ + return "nemo_msgs/Heartbeat"; +} + + +//! @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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h new file mode 100644 index 000000000..589cf8f17 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h @@ -0,0 +1,70 @@ +#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(){ + return "nemo_msgs/Progress"; +} + +//! @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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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.h b/src/comm/ros_bridge/include/messages/std_msgs/header.h new file mode 100644 index 000000000..55ac9de09 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/std_msgs/header.h @@ -0,0 +1,106 @@ +#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(){ + return "std_msgs/Header"; +} + +//! @brief C++ representation of std_msgs/Header +class Header{ +public: + using Time = std_msgs::time::Time; + Header() : _seq(_defaultSeq++), _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: + static uint32_t _defaultSeq = 0; + uint32_t _seq; + Time _stamp; + std::string _frameId; +}; + +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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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; +} + +} // namespace time +} // namespace std_msgs +} // namespace messages +} // namespace ros_bridge 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 000000000..b8d70643f --- /dev/null +++ b/src/comm/ros_bridge/include/messages/std_msgs/time.h @@ -0,0 +1,68 @@ +#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(){ + return "std_msgs/Time"; +} + +//! @brief C++ representation of std_msgs/Time +class Time{ +public: + Time(): _secs(0), _nsecs(0) {} + Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {} + 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); + value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), 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/ROSBridge.h b/src/comm/ros_bridge/include/ros_bridge.h similarity index 54% rename from src/comm/ros_bridge/include/ROSBridge.h rename to src/comm/ros_bridge/include/ros_bridge.h index 59faeeb88..0bb9e12c0 100644 --- a/src/comm/ros_bridge/include/ROSBridge.h +++ b/src/comm/ros_bridge/include/ros_bridge.h @@ -4,37 +4,29 @@ #include "ros_bridge/include/CasePacker.h" #include "ros_bridge/include/TypeFactory.h" #include "ros_bridge/include/JsonFactory.h" -#include "ros_bridge/include/TopicPublisher.h" -#include "ros_bridge/include/TopicSubscriber.h" -#include "ros_bridge/include/Server.h" +#include "ros_bridge/include/topic_publisher.h" +#include "ros_bridge/include/topic_subscriber.h" +#include "ros_bridge/include/server.h" #include #include -namespace ROSBridge { +namespace ros_bridge { class ROSBridge { -public: - typedef MessageTag Tag; +public: typedef rapidjson::Document JsonDoc; typedef std::unique_ptr JsonDocUPtr; explicit ROSBridge(); explicit ROSBridge(const char* connectionString); - - template - void publish(T &msg, const std::string &topic){ - _topicPublisher.publish(msg, topic); - } - void publish(JsonDocUPtr doc); + void publish(JsonDocUPtr doc, const char* topic); void subscribe(const char *topic, const std::function &callBack); - void advertiseService(const std::string &service, - const std::string &type, + void advertiseService(const char* service, + const char* type, const std::function &callback); - const CasePacker *casePacker() const; - //! @brief Start ROS bridge. void start(); //! @brief Stops ROS bridge. @@ -42,18 +34,14 @@ public: //! @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(); - bool isRunning(); private: - std::shared_ptr _stopped; - TypeFactory _typeFactory; - JsonFactory _jsonFactory; - CasePacker _casePacker; - RosbridgeWsClient _rbc; - ComPrivate::TopicPublisher _topicPublisher; - ComPrivate::TopicSubscriber _topicSubscriber; - ComPrivate::Server _server; + std::shared_ptr _stopped; + RosbridgeWsClient _rbc; + com_private::TopicPublisher _topicPublisher; + com_private::TopicSubscriber _topicSubscriber; + com_private::Server _server; }; diff --git a/src/comm/ros_bridge/include/Server.cpp b/src/comm/ros_bridge/include/server.cpp similarity index 91% rename from src/comm/ros_bridge/include/Server.cpp rename to src/comm/ros_bridge/include/server.cpp index a83d04bbe..c1a335a71 100644 --- a/src/comm/ros_bridge/include/Server.cpp +++ b/src/comm/ros_bridge/include/server.cpp @@ -1,8 +1,8 @@ -#include "Server.h" +#include "server.h" #include "rapidjson/include/rapidjson/ostreamwrapper.h" -ROSBridge::ComPrivate::Server::Server(CasePacker &casePacker, RosbridgeWsClient &rbc) : +ros_bridge::com_private::Server::Server(CasePacker &casePacker, RosbridgeWsClient &rbc) : _rbc(rbc) , _casePacker(casePacker) , _stopped(std::make_shared(true)) @@ -10,7 +10,7 @@ ROSBridge::ComPrivate::Server::Server(CasePacker &casePacker, RosbridgeWsClient } -void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service, +void ros_bridge::com_private::Server::advertiseService(const std::string &service, const std::string &type, const Server::CallbackType &userCallback) { @@ -74,17 +74,17 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service, }); } -ROSBridge::ComPrivate::Server::~Server() +ros_bridge::com_private::Server::~Server() { this->reset(); } -void ROSBridge::ComPrivate::Server::start() +void ros_bridge::com_private::Server::start() { _stopped->store(false); } -void ROSBridge::ComPrivate::Server::reset() +void ros_bridge::com_private::Server::reset() { if ( _stopped->load() ) return; diff --git a/src/comm/ros_bridge/include/Server.h b/src/comm/ros_bridge/include/server.h similarity index 90% rename from src/comm/ros_bridge/include/Server.h rename to src/comm/ros_bridge/include/server.h index 4f622ca54..8a8145cc6 100644 --- a/src/comm/ros_bridge/include/Server.h +++ b/src/comm/ros_bridge/include/server.h @@ -1,14 +1,14 @@ #pragma once -#include "ros_bridge/include/ComPrivateInclude.h" +#include "ros_bridge/include/com_private.h" #include "ros_bridge/include/RosBridgeClient.h" #include "ros_bridge/include/TypeFactory.h" #include "ros_bridge/include/CasePacker.h" #include -namespace ROSBridge { -namespace ComPrivate { +namespace ros_bridge { +namespace com_private { class Server { 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 000000000..a2d4aa62a --- /dev/null +++ b/src/comm/ros_bridge/include/topic_publisher.cpp @@ -0,0 +1,105 @@ +#include "topic_publisher.h" + +#include + +ros_bridge::com_private::TopicPublisher::TopicPublisher(CasePacker &casePacker, + RosbridgeWsClient &rbc) : + _stopped(std::make_shared(true)) + , _casePacker(casePacker) + , _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]{ + // Init. + std::unordered_map topicMap; + // 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; + assert(com_private::getTopic(*pJsonDoc, topic)); + assert(com_private::removeTopic(*pJsonDoc)); + std::string type; + assert(com_private::getType(*pJsonDoc, type)); + assert(com_private::removeType(*pJsonDoc)); + + // Check if topic must be advertised. + std::string clientName = + ros_bridge::com_private::_topicAdvertiserKey + + topic; + auto it = topicMap.find(clientName); + if ( it == topicMap.end()) { // Need to advertise topic? + topicMap.insert(std::make_pair(clientName, topic)); + this->_rbc.addClient(clientName); + this->_rbc.advertise(clientName, topic, type); + this->_rbc.waitForTopic(topic, [this]{ + return this->_stopped->load(); + }); // Wait until topic is advertised. + } + + // Publish Json message. + if ( !this->_stopped->load() ) + this->_rbc.publish(topic, *pJsonDoc); + } // while loop + + // Tidy up. + for (auto& it : topicMap){ + this->_rbc.unadvertise(it.second); + this->_rbc.removeClient(it.first); + } + + std::cout << "TopicPublisher: publisher thread terminated." << std::endl; + }); +} + +void 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(); + _queue.clear(); +} + +void ros_bridge::com_private::TopicPublisher::publish( + ros_bridge::com_private::JsonDocUPtr docPtr, + const char *topic){ + docPtr->AddMember("topic", topic, doc->GetAllocator()); + std::unique_lock lock(_mutex); + _queue.push_back(std::move(docPtr)); + lock.unlock(); + _cv.notify_one(); // Wake publisher thread. +} + diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/topic_publisher.h similarity index 56% rename from src/comm/ros_bridge/include/TopicPublisher.h rename to src/comm/ros_bridge/include/topic_publisher.h index 9ca78b948..eec9c8662 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.h +++ b/src/comm/ros_bridge/include/topic_publisher.h @@ -1,19 +1,15 @@ #pragma once -#include "ros_bridge/include/TypeFactory.h" -#include "ros_bridge/include/CasePacker.h" -#include "ros_bridge/include/ComPrivateInclude.h" +#include "ros_bridge/include/com_private.h" #include "ros_bridge/include/RosBridgeClient.h" #include -#include #include #include -#include #include -namespace ROSBridge { -namespace ComPrivate { +namespace ros_bridge { +namespace com_private { struct ThreadData; @@ -35,25 +31,12 @@ public: //! @brief Resets the publisher. void reset(); - void publish(JsonDocUPtr docPtr){ - { - std::lock_guard lock(_mutex); - _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)); - } + void publish(JsonDocUPtr docPtr, const char *topic); private: JsonQueue _queue; std::mutex _mutex; std::shared_ptr _stopped; - CasePacker &_casePacker; RosbridgeWsClient &_rbc; CondVar _cv; ThreadPtr _pThread; diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/topic_subscriber.cpp similarity index 76% rename from src/comm/ros_bridge/include/TopicSubscriber.cpp rename to src/comm/ros_bridge/include/topic_subscriber.cpp index d6896400c..966303bbf 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.cpp +++ b/src/comm/ros_bridge/include/topic_subscriber.cpp @@ -1,52 +1,46 @@ -#include "TopicSubscriber.h" +#include "topic_subscriber.h" #include -ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker, - RosbridgeWsClient &rbc) : - _casePacker(casePacker) - , _rbc(rbc) +ros_bridge::com_private::TopicSubscriber::TopicSubscriber(RosbridgeWsClient &rbc) : + _rbc(rbc) , _stopped(std::make_shared(true)) { } -ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber() +ros_bridge::com_private::TopicSubscriber::~TopicSubscriber() { this->reset(); } -void ROSBridge::ComPrivate::TopicSubscriber::start() +void ros_bridge::com_private::TopicSubscriber::start() { _stopped->store(false); } -void ROSBridge::ComPrivate::TopicSubscriber::reset() +void ros_bridge::com_private::TopicSubscriber::reset() { if ( !_stopped->load() ){ - std::cout << "TopicSubscriber: _stopped->store(true) " << std::endl; _stopped->store(true); { for (auto &item : _topicMap){ - std::cout << "TopicSubscriber: unsubscribe " << item.second << std::endl; _rbc.unsubscribe(item.second); - std::cout << "TopicSubscriber: removeClient " << item.first << std::endl; _rbc.removeClient(item.first); } } _topicMap.clear(); - std::cout << "TopicSubscriber: _topicMap cleared " << std::endl; } } -void ROSBridge::ComPrivate::TopicSubscriber::subscribe( +void ros_bridge::com_private::TopicSubscriber::subscribe( const char *topic, const std::function &callback) { if ( _stopped->load() ) return; - std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey + std::string clientName = ros_bridge::com_private::_topicSubscriberKey + std::string(topic); auto it = _topicMap.find(clientName); if ( it != _topicMap.end() ){ // Topic already subscribed? @@ -55,7 +49,6 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe( _topicMap.insert(std::make_pair(clientName, std::string(topic))); // Wrap callback. - using namespace std::placeholders; auto callbackWrapper = [callback]( std::shared_ptr, std::shared_ptr in_message) diff --git a/src/comm/ros_bridge/include/TopicSubscriber.h b/src/comm/ros_bridge/include/topic_subscriber.h similarity index 70% rename from src/comm/ros_bridge/include/TopicSubscriber.h rename to src/comm/ros_bridge/include/topic_subscriber.h index 5aae3cd51..eff43217d 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.h +++ b/src/comm/ros_bridge/include/topic_subscriber.h @@ -1,14 +1,12 @@ #pragma once -#include "ros_bridge/include/ComPrivateInclude.h" +#include "ros_bridge/include/com_private.h" #include "ros_bridge/include/RosBridgeClient.h" -#include "ros_bridge/include/TypeFactory.h" -#include "ros_bridge/include/CasePacker.h" #include -namespace ROSBridge { -namespace ComPrivate { +namespace ros_bridge { +namespace com_private { typedef std::function CallbackType; @@ -19,7 +17,7 @@ class TopicSubscriber public: TopicSubscriber() = delete; - TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc); + TopicSubscriber(RosbridgeWsClient &rbc); ~TopicSubscriber(); //! @brief Starts the subscriber. @@ -34,11 +32,7 @@ public: void subscribe(const char* topic, const CallbackType &callback); private: - - - - CasePacker &_casePacker; - RosbridgeWsClient &_rbc; + RosbridgeWsClient &_rbc; TopicMap _topicMap; std::shared_ptr _stopped; }; diff --git a/src/comm/ros_bridge/src/CasePacker.cpp b/src/comm/ros_bridge/src/CasePacker.cpp deleted file mode 100644 index cd17974f3..000000000 --- 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/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp deleted file mode 100644 index 40983a1c0..000000000 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include "ros_bridge/include/ROSBridge.h" - -ROSBridge::ROSBridge::ROSBridge(const char *connectionString) : - _stopped(std::make_shared(true)) - , _casePacker(&_typeFactory, &_jsonFactory) - , _rbc(connectionString, false /*run*/) - , _topicPublisher(_casePacker, _rbc) - , _topicSubscriber(_casePacker, _rbc) - , _server(_casePacker, _rbc) -{ -} - -ROSBridge::ROSBridge::ROSBridge() : - ROSBridge::ROSBridge::ROSBridge("localhost:9090") -{ -} - -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); -} - -void ROSBridge::ROSBridge::advertiseService(const std::string &service, - const std::string &type, - const std::function &callback) -{ - _server.advertiseService(service, type, callback); -} - -const ROSBridge::CasePacker *ROSBridge::ROSBridge::casePacker() const -{ - return &_casePacker; -} - -void ROSBridge::ROSBridge::start() -{ - _stopped->store(false); - _rbc.run(); - _topicPublisher.start(); - _topicSubscriber.start(); - _server.start(); -} - -void ROSBridge::ROSBridge::reset() -{ - auto start = std::chrono::high_resolution_clock::now(); - _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 << "ROSBridge reset duration: " << delta << " ms" << std::endl; -} - -bool ROSBridge::ROSBridge::connected() -{ - return _rbc.connected(); -} - -bool ROSBridge::ROSBridge::isRunning() -{ - return !_stopped->load(); -} - - -bool ROSBridge::isValidConnectionString(const char *connectionString) -{ - return is_valid_port_path(connectionString); -} 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 000000000..cbf0bf93d --- /dev/null +++ b/src/comm/ros_bridge/src/ros_bridge.cpp @@ -0,0 +1,71 @@ +#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::start() +{ + _stopped->store(false); + _rbc.run(); + _topicPublisher.start(); + _topicSubscriber.start(); + _server.start(); +} + +void ros_bridge::ROSBridge::reset() +{ + auto start = std::chrono::high_resolution_clock::now(); + _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() +{ + 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 a3556846a..96a4c74c9 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 -- 2.22.0