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