diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 67fe961d5935543a5b6dcf56408a9d242803ad67..bb3f7f83074c787437223743d1dfcf4c7c0d6f90 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -430,6 +430,7 @@ HEADERS += \ src/Snake/snake_geometry.h \ src/Snake/snake_global.h \ src/Wima/GeoPoint3D.h \ + src/Wima/NemoProgress.h \ src/Wima/Polygon2D.h \ src/Wima/PolygonArray.h \ src/Wima/QtROSJsonFactory.h \ @@ -472,9 +473,13 @@ HEADERS += \ src/Wima/testplanimetrycalculus.h \ src/Settings/WimaSettings.h \ src/QmlControls/QmlObjectVectorModel.h \ + src/comm/ros_bridge/include/GenericMessages.h \ src/comm/ros_bridge/include/JsonMethodes.h \ src/comm/ros_bridge/include/MessageTraits.h \ + src/comm/ros_bridge/include/PackageBuffer.h \ src/comm/ros_bridge/include/TypeFactory.h \ + src/comm/ros_bridge/src/CasePacker.h \ + src/comm/ros_bridge/src/PackageBuffer.h \ src/comm/utilities.h SOURCES += \ src/Snake/clipper/clipper.cpp \ @@ -482,6 +487,7 @@ SOURCES += \ src/Snake/snake_geometry.cpp \ src/Wima/GeoPoint3D.cpp \ src/Wima/PolygonArray.cc \ + src/comm/ros_bridge/src/CasePacker.cpp \ src/comm/ros_bridge/src/ROSCommunicator.cpp \ src/Wima/WimaControllerDetail.cc \ src/Wima/snaketile.cpp \ diff --git a/src/Wima/NemoProgress.h b/src/Wima/NemoProgress.h new file mode 100644 index 0000000000000000000000000000000000000000..6270795886ff2751b26db3b37e7c1734d45953d0 --- /dev/null +++ b/src/Wima/NemoProgress.h @@ -0,0 +1,7 @@ +#pragma once +#include + +#include "ros_bridge/include/JsonMethodes.h" + +namespace ProgressNS = ROSBridge::JsonMethodes::Progress; +typedef ProgressNS::GenericProgress NemoProgress; diff --git a/src/Wima/Polygon2D.h b/src/Wima/Polygon2D.h index e8567ba863330e5f9e35900986faafdad480faee..67ba05aed1910fca8452b316c8b9d1119f8a1c3a 100644 --- a/src/Wima/Polygon2D.h +++ b/src/Wima/Polygon2D.h @@ -5,27 +5,39 @@ #include "ros_bridge/include/MessageGroups.h" #include "ros_bridge/include/MessageBaseClass.h" +#include "ros_bridge/include/JsonMethodes.h" + +namespace MsgGroupsNS = ROSBridge::MessageGroups; +namespace PolyStampedNS = ROSBridge::JsonMethodes::PolygonStamped; typedef ROSBridge::MessageBaseClass ROSMsg; -namespace MsgGroups = ROSBridge::MessageGroups; -class Polygon2D : public QPolygonF, public ROSMsg{ +template class ContainerType = QVector> +class Polygon2DTemplate : public ROSMsg{ // + typedef ROSBridge::JsonMethodes::Polygon::Polygon Poly; public: - typedef MsgGroups::PolygonGroup Group; + typedef MsgGroupsNS::PolygonStampedGroup Group; // has no header - Polygon2D(){} - Polygon2D(const Polygon2D &other) : QPolygonF(other) , ROSMsg(){} + Polygon2DTemplate(){} + Polygon2DTemplate(const Polygon2DTemplate &other) : ROSMsg(), _polygon(other._polygon){} - Polygon2D& operator=(const Polygon2D& other) { - QPolygonF::operator=(other); + Polygon2DTemplate& operator=(const Polygon2DTemplate& other) { + this->_polygon = other._polygon; return *this; } - virtual Polygon2D*Clone() const { // ROSMsg - return new Polygon2D(*this); - } + const Poly &polygon() const {return _polygon;} + Poly &polygon() {return _polygon;} - const Polygon2D &points() const {return *this;} // ROSMsg - Polygon2D &points() {return *this;} // ROSMsg + const ContainerType &path() const {return _polygon.points();} + ContainerType &path() {return _polygon.points();} + + virtual Polygon2DTemplate*Clone() const { // ROSMsg + return new Polygon2DTemplate(*this); + } +private: + Poly _polygon; }; + +typedef Polygon2DTemplate<> Polygon2D; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index d6f7eddbfd51fd96607a9c84574edc125edfd275..bc0317085e79891b02821caaffadfa7067d4c1c8 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -7,6 +7,7 @@ #include "QtROSJsonFactory.h" #include "QtROSTypeFactory.h" +#include "NemoProgress.h" #include "time.h" #include "assert.h" @@ -609,7 +610,7 @@ bool WimaController::_fetchContainerData() Polygon2D Tile; for ( const auto &vertex : tile.outer()) { QPointF QVertex(vertex.get<0>(), vertex.get<1>()); - Tile.append(QVertex); + Tile.path().append(QVertex); } _snakeTilesLocal.polygons().append(Tile); } @@ -649,7 +650,67 @@ bool WimaController::_fetchContainerData() TypeFactory.create(*doc.data(), tiles_same); + Polygon2D tile1; + tile1.path().push_back(QPointF(1,0)); + tile1.path().push_back(QPointF(1,1)); + tile1.path().push_back(QPointF(1,2)); + Polygon2D tile2; + tile2.path().push_back(QPointF(2,0)); + tile2.path().push_back(QPointF(2,1)); + tile2.path().push_back(QPointF(2,2)); + Polygon2D tile3; + tile3.path().push_back(QPointF(3,0)); + tile3.path().push_back(QPointF(3,1)); + tile3.path().push_back(QPointF(3,2)); + SnakeTilesLocal tilesSmall; + tilesSmall.polygons().push_back(tile1); + tilesSmall.polygons().push_back(tile2); + tilesSmall.polygons().push_back(tile3); + + + QScopedPointer jsonTileSmall(JsonFactory.create(tilesSmall)); + + SnakeTilesLocal tilesSmallSame; + TypeFactory.create(*jsonTileSmall.data(), tilesSmallSame); + + QScopedPointer jsonTileSmallSame(JsonFactory.create(tilesSmallSame)); + + cout << "Snake Tiles small" << endl; + rapidjson::Writer writer4(out); + jsonTileSmall->Accept(writer4); + std::cout << std::endl << std::endl; + + cout << "Snake Tiles small same" << endl; + rapidjson::Writer writer5(out); + jsonTileSmallSame->Accept(writer5); + std::cout << std::endl << std::endl; + + // progress + NemoProgress progress; + progress.progress().push_back(1); + progress.progress().push_back(2); + progress.progress().push_back(3); + progress.progress().push_back(4); + progress.progress().push_back(13); + progress.progress().push_back(600); + + + QScopedPointer jsonProgress(JsonFactory.create(progress)); + + NemoProgress progressSame; + TypeFactory.create(*jsonProgress.data(), progressSame); + + QScopedPointer jsonProgressSame(JsonFactory.create(progressSame)); + + cout << "Snake Tiles small" << endl; + rapidjson::Writer writer6(out); + jsonProgress->Accept(writer6); + std::cout << std::endl << std::endl; + cout << "Snake Tiles small same" << endl; + rapidjson::Writer writer7(out); + jsonProgressSame->Accept(writer7); + std::cout << std::endl << std::endl; } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 3804c84fff6e694383aa3574ad26015e89bd7b4e..2d5454c2ff8c6647844ca3b0403d39e8c56de51c 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -26,8 +26,6 @@ #include "snake.h" #include "WimaControllerDetail.h" -//#include "snaketile.h" -//#include "Polygon2D.h" #include "SnakeTiles.h" #include "SnakeTilesLocal.h" #include "GeoPoint3D.h" diff --git a/src/comm/ros_bridge/include/GenericMessages.h b/src/comm/ros_bridge/include/GenericMessages.h new file mode 100644 index 0000000000000000000000000000000000000000..ea71ecafc4544c2b990a5e710b591f3d4721f877 --- /dev/null +++ b/src/comm/ros_bridge/include/GenericMessages.h @@ -0,0 +1,331 @@ +#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) {} + + 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) {} + + 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);} + +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) + {} + + _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: + GenericProgress(){} + GenericProgress(const ContainterType &progress) :_progress(progress){} + + const ContainterType &progress(void) const {return _progress;} + ContainterType &progress(void) {return _progress;} +private: + ContainterType _progress; +}; + +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 index ebad92bc12ed678f749623e139269ae3e5ba42cd..a6346a0b2f413570065531afb9c7a8be300261a3 100644 --- a/src/comm/ros_bridge/include/JsonFactory.h +++ b/src/comm/ros_bridge/include/JsonFactory.h @@ -4,9 +4,10 @@ #include "ros_bridge/include/JsonMethodes.h" #include "MessageBaseClass.h" #include "utilities.h" -#include "ros_bridge/include/MessageTraits.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" @@ -21,13 +22,13 @@ class StdHeaderPolicy; //! 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 +template class JsonFactory : public HeaderPolicy { - typedef MessageBaseClass ROSMsg; + typedef MessageBaseClass ROSMsg; public: - JsonFactory(){} + JsonFactory() : HeaderPolicy() {} //! //! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg. @@ -64,7 +65,7 @@ private: rapidjson::Document *_create(const U &msg, Type2Type){ using namespace ROSBridge; rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType); - bool ret = JsonMethodes::Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator()); + bool ret = JsonMethodes::GeometryMsgs::Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator()); assert(ret); (void)ret; @@ -78,7 +79,7 @@ private: rapidjson::Document *_create(const U &msg, Type2Type){ using namespace ROSBridge; rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType); - bool ret = JsonMethodes::GeoPoint::toJson(msg, *doc, doc->GetAllocator()); + bool ret = JsonMethodes::GeographicMsgs::GeoPoint::toJson(msg, *doc, doc->GetAllocator()); assert(ret); (void)ret; @@ -92,7 +93,7 @@ private: rapidjson::Document *_create(const U &msg, Type2Type){ using namespace ROSBridge; rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType); - bool ret = JsonMethodes::Polygon::toJson(msg, *doc, doc->GetAllocator()); + bool ret = JsonMethodes::GeometryMsgs::Polygon::toJson(msg, *doc, doc->GetAllocator()); assert(ret); (void)ret; @@ -115,7 +116,7 @@ private: 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::PolygonStamped::toJson(msg, *doc, doc->GetAllocator()); + bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg, *doc, doc->GetAllocator()); assert(ret); (void)ret; @@ -128,7 +129,7 @@ private: using namespace ROSBridge; JsonMethodes::Header::Header header(HeaderPolicy::header(msg)); rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType); - bool ret = JsonMethodes::PolygonStamped::toJson(msg, header, *doc, doc->GetAllocator()); + bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg.polygon(), header, *doc, doc->GetAllocator()); assert(ret); (void)ret; @@ -151,7 +152,7 @@ private: 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::PolygonArray::toJson(msg, *doc, doc->GetAllocator()); + bool ret = JsonMethodes::JSKRecognitionMsg::PolygonArray::toJson(msg, *doc, doc->GetAllocator()); assert(ret); (void)ret; @@ -164,17 +165,30 @@ private: using namespace ROSBridge; JsonMethodes::Header::Header header(HeaderPolicy::header(msg)); rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType); - bool ret = JsonMethodes::PolygonArray::toJson(msg, header, *doc, doc->GetAllocator()); + bool ret = JsonMethodes::JSKRecognitionMsg::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; + } }; class StdHeaderPolicy{ + namespace StdMsgs = ROSBridge::GenericMessages::StdMsgs; public: StdHeaderPolicy():_seq(-1){}; @@ -183,8 +197,8 @@ public: //! \return Returns the header belonging to msg. //! template - ROSBridge::JsonMethodes::Header::Header header(const T&msg) { - return ROSBridge::JsonMethodes::Header::Header(++_seq, time(msg), "/map"); + StdMsgs::Header header(const T&msg) { + return StdMsgs::Header(++_seq, time(msg), "/map"); } @@ -192,9 +206,9 @@ public: //! \brief time Returns the current time. //! \return Returns the current time. template - ROSBridge::JsonMethodes::Time::Time time(const T&msg) { + StdMsgs::Time time(const T&msg) { (void)msg; - return ROSBridge::JsonMethodes::Time::Time(0,0); + return StdMsgs::Time(0,0); } private: long _seq; diff --git a/src/comm/ros_bridge/include/JsonMethodes.h b/src/comm/ros_bridge/include/JsonMethodes.h index eaa36f815040ee297981c12b49762a8e80d4341d..810983ff8026fdf6226af7844255c799df44d234 100644 --- a/src/comm/ros_bridge/include/JsonMethodes.h +++ b/src/comm/ros_bridge/include/JsonMethodes.h @@ -14,111 +14,62 @@ #include namespace ROSBridge { -namespace JsonMethodes { - -typedef std::ostream OStream; - -namespace Time { - //! @brief C++ representation of std_msgs/Time - class Time{ - public: - Time(): _secs(0), _nsecs(0) {} - Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {} - - 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 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::Document &doc, + rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) { - doc.AddMember("secs", rapidjson::Value().SetUint((uint32_t)time.secs()), allocator); - doc.AddMember("nsecs", rapidjson::Value().SetUint((uint32_t)time.nSecs()), 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 JsonType &doc, + template + bool fromJson(const rapidjson::Value &value, TimeType &time) { - if (!doc.HasMember("secs") || !doc["secs"].IsUint()) + if (!value.HasMember("secs") || !value["secs"].IsUint()){ + assert(false); return false; - if (!doc.HasMember("nsecs")|| !doc["nsecs"].IsUint()) + } + if (!value.HasMember("nsecs")|| !value["nsecs"].IsUint()){ + assert(false); return false; - time.setSecs(doc["secs"].GetUint()); - time.setNSecs(doc["nsecs"].GetUint()); + } + time.setSecs(value["secs"].GetUint()); + time.setNSecs(value["nsecs"].GetUint()); return true; } - template - bool fromJson(const rapidjson::Document &doc, - TimeType &time) - { - return _fromJson(doc, time); - } - - template - bool fromJson(const rapidjson::Value &doc, - TimeType &time) - { - return _fromJson(doc, time); - } - -} +} // Time +//! @brief Namespace containing methodes for std_msgs/Header message generation. namespace Header { - - - //! @brief C++ representation of std_msgs/Header - class Header{ - public: - Header() : _seq(0), _stamp(Time::Time()), _frameId("") {} - Header(uint32_t seq, const Time::Time &stamp, const std::string &frame_id) : - _seq(seq) - , _stamp(stamp) - , _frameId(frame_id) {} - - uint32_t seq() const {return _seq;}; - const Time::Time &stamp() const {return _stamp;}; - const std::string &frameId() const {return _frameId;}; - - Time::Time &stamp() {return _stamp;}; - std::string &frameId() {return _frameId;}; - - void setSeq (uint32_t seq) {_seq = seq;} - void setStamp (const Time::Time &stamp) {_stamp = stamp;} - void setFrameId (const std::string &frameId) {_frameId = frameId;} - private: - uint32_t _seq; - Time::Time _stamp; - std::string _frameId; - }; - - template bool toJson(const HeaderType &header, - rapidjson::Document &doc, + rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) { - doc.AddMember("seq", rapidjson::Value().SetUint((uint32_t)header.seq()), allocator); + value.AddMember("seq", rapidjson::Value().SetUint((uint32_t)header.seq()), allocator); - rapidjson::Document stamp(rapidjson::kObjectType); - if (!Time::toJson(header.stamp(), doc, allocator)) - return false; - doc.AddMember("stamp", stamp, allocator); + rapidjson::Value stamp(rapidjson::kObjectType); + if (!Time::toJson(header.stamp(), stamp, allocator)){ + assert(false); + return false; + } + value.AddMember("stamp", stamp, allocator); - doc.AddMember("frame_id", + value.AddMember("frame_id", rapidjson::Value().SetString(header.frameId().data(), header.frameId().length(), allocator), @@ -127,69 +78,43 @@ namespace Header { return true; } - template - bool _fromJson(const JsonType &doc, + template + bool fromJson(const rapidjson::Value &value, HeaderType &header) { - if (!doc.HasMember("seq")|| !doc["seq"].IsUint()) + if (!value.HasMember("seq")|| !value["seq"].IsUint()){ + assert(false); return false; - if (!doc.HasMember("stamp")) + } + if (!value.HasMember("stamp")){ + assert(false); return false; - if (!doc.HasMember("frame_id")|| !doc["frame_id"].IsString()) + } + if (!value.HasMember("frame_id")|| !value["frame_id"].IsString()){ + assert(false); return false; - header.setSeq(doc["seq"].GetUint()); + } + header.setSeq(value["seq"].GetUint()); decltype(header.stamp()) time; - if (!Time::fromJson(doc["stamp"], time)) + if (!Time::fromJson(value["stamp"], time)){ + assert(false); return false; + } header.setStamp(time); - header.setFrameId(doc["frame_id"].GetString()); + header.setFrameId(value["frame_id"].GetString()); return true; } +} // Header +} // StdMsgs - template - bool fromJson(const rapidjson::Value &doc, - HeaderType &header) - { - return _fromJson(doc, header); - } - - template - bool fromJson(const rapidjson::Document &doc, - HeaderType &header) - { - return _fromJson(doc, header); - } - -} +//! @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; - //! @brief C++ representation of geometry_msgs/Point32. - template - class Point{ - public: - Point(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;} - - - private: - FloatType _x; - FloatType _y; - FloatType _z; - }; - - typedef Point<> Point64; - typedef Point<_Float32> Point32; - namespace det { //detail template @@ -222,104 +147,177 @@ using namespace ROSBridge::traits; } template - bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) + bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) { - doc.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator); - doc.AddMember("y", rapidjson::Value().SetFloat(p.y()), 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. - doc.AddMember("y", rapidjson::Value().SetFloat(z), allocator); + value.AddMember("z", rapidjson::Value().SetFloat(z), allocator); return true; } - template - bool _fromJson(const JsonType &doc, + template + bool fromJson(const rapidjson::Value &value, PointType &p) { - if (!doc.HasMember("x") || !doc["x"].IsFloat()) + if (!value.HasMember("x") || !value["x"].IsFloat()){ + assert(false); return false; - if (!doc.HasMember("y") || !doc["y"].IsFloat()) + } + if (!value.HasMember("y") || !value["y"].IsFloat()){ + assert(false); return false; - if (!doc.HasMember("z") || !doc["z"].IsFloat()) + } + if (!value.HasMember("z") || !value["z"].IsFloat()){ + assert(false); return false; + } - p.setX(doc["x"].GetFloat()); - p.setY(doc["y"].GetFloat()); + 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(doc["z"], p, Type2Type()); // If PointType has no member z() discard doc["z"]. + (void)det::setZ(value["z"], p, Type2Type()); // If PointType has no member z() discard doc["z"]. return true; } +} // Point32 - template - bool fromJson(const rapidjson::Value &doc, - PointType &point) - { - return _fromJson(doc, point); +//! @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 < (unsigned long)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); + } - template - bool fromJson(const rapidjson::Document &doc, - PointType &point) - { - return _fromJson(doc, point); + 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().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); } -namespace GeoPoint { -using namespace ROSBridge::traits; +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); - //! @brief C++ representation of geographic_msgs/GeoPoint. - class GeoPoint{ - public: - GeoPoint() - : _latitude(0) - , _longitude(0) - , _altitude(0) - {} - GeoPoint(_Float64 latitude, _Float64 longitude, _Float64 altitude) - : _latitude(latitude) - , _longitude(longitude) - , _altitude(altitude) - {} - - _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;} - - - - bool operator==(const GeoPoint &p1) { - return (p1._latitude == this->_latitude - && p1._longitude == this->_longitude - && p1._altitude == this->_altitude); - } + return true; +} - bool operator!=(const GeoPoint &p1) { - return !this->operator==(p1); - } +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; + } - friend std::ostream& operator<<(std::ostream& os, const GeoPoint& p) - { - os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude << "\n"; - return os; - } - private: - _Float64 _latitude; - _Float64 _longitude; - _Float64 _altitude; - }; + 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 @@ -348,279 +346,60 @@ using namespace ROSBridge::traits; (void)doc; (void)p; } - } + } // namespace det template - bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) + bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) { - doc.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator); - doc.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), 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; - doc.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator); + value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator); return true; } - template - bool _fromJson(const JsonType &doc, + template + bool fromJson(const rapidjson::Value &value, PointType &p) { - if (!doc.HasMember("latitude") || !doc["latitude"].IsFloat()) + if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){ + assert(false); return false; - if (!doc.HasMember("longitude") || !doc["longitude"].IsFloat()) + } + if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){ + assert(false); return false; - if (!doc.HasMember("altitude") || !doc["altitude"].IsFloat()) + } + if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){ + assert(false); return false; + } - p.setLatitude(doc["latitude"].GetFloat()); - p.setLongitude(doc["longitude"].GetFloat()); + 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(doc["altitude"], p, Type2Type()); // If T has no member altitude() discard doc["altitude"]; + det::setAltitude(value["altitude"], p, Type2Type()); // If T has no member altitude() discard doc["altitude"]; return true; } - template - bool fromJson(const rapidjson::Value &doc, - PointType &point) - { - return _fromJson(doc, point); - } - - template - bool fromJson(const rapidjson::Document &doc, - PointType &point) - { - return _fromJson(doc, point); - } - -} - -namespace Polygon { - - //! @brief C++ representation of geometry_msgs/Polygon - template class ContainerType = std::vector> - class Polygon{ - typedef typename boost::remove_cv::type>::type PointType; - public: - Polygon(){} - Polygon(const Polygon &poly) : _points(poly.points()){} - - - const ContainerType &points() const {return _points;} - ContainerType &points() {return _points;} - - private: - ContainerType _points; - }; - - template - bool toJson(const PolygonType &poly, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) - { - rapidjson::Value points(rapidjson::kArrayType); - - for(unsigned long i=0; i < (unsigned long)poly.points().size(); ++i) { - rapidjson::Document point(rapidjson::kObjectType); - if ( !Point32::toJson(poly.points()[i], point, allocator) ) - return false; - points.PushBack(point, allocator); - } - - doc.AddMember("points", points, allocator); - - return true; - } - - template - bool _fromJson(const JsonType &doc, PolygonType &poly) - { - if (!doc.HasMember("points") || !doc["points"].IsArray()) - return false; - const auto &array = doc["points"].GetArray(); - for (long i=0; i < array.Size(); ++i){ - if ( !Point32::fromJson(array[i], poly.points()[i]) ) - return false; - } - return true; - } - - template - bool fromJson(const rapidjson::Value &doc, - PolygonType &poly) - { - return _fromJson(doc, poly); - } - - template - bool fromJson(const rapidjson::Document &doc, - PolygonType &poly) - { - return _fromJson(doc, poly); - } - - -} - -namespace PolygonStamped { - - - //! @brief C++ representation of geometry_msgs/PolygonStamped - template class PolygonType = Polygon::Polygon, class HeaderType = Header::Header> - class PolygonStamped { - typedef typename boost::remove_cv::type>::type PointType; - typedef PolygonType Polygon; - public: - PolygonStamped(){}; - PolygonStamped(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;} - - void setHeader(const HeaderType &header) {_header = header;} - void setPolygon(const Polygon &polygon) - { - _polygon = polygon; - } - - private: - HeaderType _header; - Polygon _polygon; - }; - - - template - bool toJson(const PolyStamped &polyStamped, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) - { - return toJson(polyStamped.polygon(), polyStamped.header(), doc, allocator); - } - - template - bool toJson(const PolygonType &poly, const HeaderType &h, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) - { - rapidjson::Document header(rapidjson::kObjectType); - if (!Header::toJson(h, header, allocator)) - return false; - rapidjson::Document polygon(rapidjson::kObjectType); - if (!Polygon::toJson(poly, polygon, allocator)) - return false; - doc.AddMember("header", header, allocator); - doc.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()) HeaderType; - HeaderType header; - bool ret = Header::fromJson(doc, header); - polyStamped.setHeader(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; - } - } - - template - bool _fromJson(const JsonType &doc, PolygonType &polyStamped) - { - if ( !doc.HasMember("header") ) - return false; - if ( !doc.HasMember("polygon") ) - return false; - +} // GeoPoint +} // GeographicMsgs - typedef traits::HasMemberSetHeader HasHeader; - if ( !det::setHeader(doc["header"], polyStamped, Int2Type())) - return false; - - if ( !Polygon::fromJson(doc["polygon"], polyStamped.polygon()) ) - return false; - - return true; - } - - template - bool fromJson(const rapidjson::Value &doc, - PolygonType &poly) - { - return _fromJson(doc, poly); - } - - template - bool fromJson(const rapidjson::Document &doc, - PolygonType &poly) - { - return _fromJson(doc, poly); - } - -} +//! @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; - - //! @brief C++ representation of jsk_recognition_msgs/PolygonArray - template class Polygon = Polygon::Polygon, - template class ContainerType = std::vector, - class HeaderType = Header::Header> - class PolygonArray{ - typedef typename boost::remove_cv::type>::type PointType; - typedef Polygon PolygonType; - public: - PolygonArray(){} - PolygonArray(const HeaderType &header, - const ContainerType &polygons, - const ContainerType &labels, - const ContainerType<_Float32> &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<_Float32> &likelyhood() const {return _likelihood;} - ContainerType<_Float32> &likelyhood() {return _likelihood;} - - void setHeader (const HeaderType &header) {_header = &header;} - void setPolygons (ContainerType &polygon) { _polygons = polygon; } - void setLabels (const ContainerType &labels) {_labels = labels;} - void setLikelihood (const ContainerType<_Float32> &likelihood) {_likelihood = likelihood;} - - - - private: - HeaderType _header; - ContainerType _polygons; - ContainerType _labels; - ContainerType<_Float32> _likelihood; - }; - +using namespace ROSBridge::JsonMethodes::StdMsgs; +using namespace ROSBridge::JsonMethodes::GeometryMsgs; namespace PAdetail { //! Helper functions to generate Json entries for labels and likelihood. @@ -695,33 +474,37 @@ using namespace ROSBridge::traits; //! \note If this function is called p.polygons[i] (entries implement the the PolygonStampedGroup interface) //! must contain a header. template - bool toJson(const PolygonArrayType &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) + bool toJson(const PolygonArrayType &pArray, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) { rapidjson::Document header(rapidjson::kObjectType); - if (Header::toJson(p.header(), header, allocator)) + if (Header::toJson(pArray.header(), header, allocator)){ + assert(false); return false; - doc.AddMember("header", header, allocator); + } + value.AddMember("header", header, allocator); rapidjson::Value polygons(rapidjson::kArrayType); - for(unsigned long i=0; i < p.polygons().size(); ++i){ + for(unsigned long i=0; i < pArray.polygons().size(); ++i){ rapidjson::Document polygon(rapidjson::kObjectType); - if (!PolygonStamped::toJson(p.polygons[i], polygon, allocator)) + if (!PolygonStamped::toJson(pArray.polygons()[i], polygon, allocator)){ + assert(false); return false; + } polygons.PushBack(polygon, allocator); } - doc.AddMember("polygons", polygons, allocator); + value.AddMember("polygons", polygons, allocator); rapidjson::Value labels(rapidjson::kArrayType); typedef HasMemberLabels HasLabels; - PAdetail::labelsToJson(p, labels, allocator, Int2Type()); - doc.AddMember("labels", labels, allocator); + PAdetail::labelsToJson(pArray, labels, allocator, Int2Type()); + value.AddMember("labels", labels, allocator); rapidjson::Value likelihood(rapidjson::kArrayType); typedef HasMemberLikelihood HasLikelihood; - PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type()); - doc.AddMember("likelihood", likelihood, allocator); + PAdetail::likelihoodToJson(pArray, likelihood, allocator, Int2Type()); + value.AddMember("likelihood", likelihood, allocator); return true; } @@ -740,94 +523,138 @@ using namespace ROSBridge::traits; //! \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::Document &doc, rapidjson::Document::AllocatorType &allocator) + 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)) + if (!Header::toJson(h, header, allocator)){ + assert(false); return false; - doc.AddMember("header", header, allocator); + } + 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], h, polygon, allocator)) + if (!PolygonStamped::toJson(p.polygons()[i].polygon(), h, polygon, allocator)){ + assert(false); return false; + } polygons.PushBack(polygon, allocator); } - doc.AddMember("polygons", polygons, allocator); + value.AddMember("polygons", polygons, allocator); rapidjson::Value labels(rapidjson::kArrayType); typedef HasMemberLabels HasLabels; PAdetail::labelsToJson(p, labels, allocator, Int2Type()); - doc.AddMember("labels", labels, allocator); + value.AddMember("labels", labels, allocator); rapidjson::Value likelihood(rapidjson::kArrayType); typedef HasMemberLikelihood HasLikelihood; PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type()); - doc.AddMember("likelihood", likelihood, allocator); + value.AddMember("likelihood", likelihood, allocator); return true; } - template - bool _fromJson(const JsonType &doc, PolygonArrayType &p) + template + bool fromJson(const rapidjson::Value &value, PolygonArrayType &p) { - if ( !doc.HasMember("header")) + if ( !value.HasMember("header")){ + assert(false); return false; - if ( !doc.HasMember("polygons") || !doc["polygons"].IsArray() ) + } + if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){ + assert(false); return false; - if ( !doc.HasMember("labels") || !doc["labels"].IsArray() ) + } + if ( !value.HasMember("labels") || !value["labels"].IsArray() ){ + assert(false); return false; - if ( !doc.HasMember("likelihood") || !doc["likelihood"].IsArray() ) + } + if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){ + assert(false); return false; + } - typedef traits::HasMemberSetHeader HasHeader; - if ( !PolygonStamped::det::setHeader(doc["header"], p, Int2Type())) + typedef traits::HasMemberHeader HasHeader; + if ( !PolygonStamped::det::setHeader(value["header"], p, Int2Type())){ + assert(false); return false; + } - const auto &polygons = doc["polygons"]; - p.polygons().reserve(polygons.Size()); - for (long i=0; i < polygons.Size(); ++i) { - if ( !polygons[i].HasMember("header") ) + const auto &polyStampedJson = value["polygons"]; + p.polygons().reserve(polyStampedJson.Size()); + typedef decltype (p.polygons()[0]) PolyStampedCVR; + typedef typename boost::remove_cv::type>::type + PolyStamped; + for (long i=0; i < polyStampedJson.Size(); ++i) { + if ( !polyStampedJson[i].HasMember("header") ){ + assert(false); return false; - typedef traits::HasMemberSetHeader HasHeader; - if ( !PolygonStamped::det::setHeader(polygons[i]["header"], p.polygons[i]().header(), Int2Type())) + } + PolyStamped polyStamped; + + if ( !PolygonStamped::det::setHeader(polyStampedJson[i]["header"], polyStamped, Int2Type())){ + assert(false); return false; + } - if ( !Polygon::fromJson(polygons[i], p.polygons[i].points())) + if ( !Polygon::fromJson(polyStampedJson[i]["polygon"], polyStamped.polygon())){ + assert(false); return false; - } + } - typedef traits::HasMemberSetLabels HasLabels; - PAdetail::setLabels(doc["labels"], p, Int2Type()); + p.polygons().push_back(std::move(polyStamped)); + } - typedef traits::HasMemberSetLikelihood HasLikelihood; - PAdetail::setLikelihood(doc["likelihood"], p, Int2Type()); + typedef traits::HasMemberLabels HasLabels; + PAdetail::setLabels(value["labels"], p, Int2Type()); + typedef traits::HasMemberLikelihood HasLikelihood; + PAdetail::setLikelihood(value["likelihood"], p, Int2Type()); return true; } - template - bool fromJson(const rapidjson::Value &doc, - PolygonArrayType &polyArray) +} // 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) { - return _fromJson(doc, polyArray); + rapidjson::Value progressJson(rapidjson::kArrayType); + for(unsigned long i=0; i < p.progress().size(); ++i){ + progressJson.PushBack(rapidjson::Value().SetInt(std::int8_t(p.progress()[i])), allocator); + } + value.AddMember("progress", progressJson, allocator); + } - template - bool fromJson(const rapidjson::Document &doc, - PolygonArrayType &polyArray) + template + bool fromJson(const rapidjson::Value &value, ProgressType &p) { - return _fromJson(doc, polyArray); - } + if (!value.HasMember("progress") || !value["progress"].IsArray()){ + assert(false); + return false; + } -} -}// end namespace JsonMethodes -} // end namespace ROSBridge + unsigned long sz = value.Size(); + p.progress().reserve(sz); + for (unsigned long i=0; i < sz; ++i) + p.progress().push_back(value[i]); + } +} // namespace Progress +} // 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 index d7e9baefc0963811cb39beae2ef65a82df8fa475..973fc22b946fc51487948e0a2a7916b81c972aaa 100644 --- a/src/comm/ros_bridge/include/MessageBaseClass.h +++ b/src/comm/ros_bridge/include/MessageBaseClass.h @@ -12,7 +12,6 @@ namespace ROSBridge { //! 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. -template class MessageBaseClass{ public: typedef MessageGroups::EmptyGroup Group; @@ -25,5 +24,7 @@ public: 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 index 31bb4625baf379d9b1fac4f29f8d73a2bad26bf1..57331a3d89041a1ab3c4d0ad53d6ff8ff1f94fb4 100644 --- a/src/comm/ros_bridge/include/MessageGroups.h +++ b/src/comm/ros_bridge/include/MessageGroups.h @@ -10,12 +10,20 @@ typedef std::string StringType; template struct MessageGroup { - static StringType label() {return _label();} + static StringType messageNameFull() {return _full();} template - static StringType _label() {return G::label()+ "/" + _label(); } + static StringType _full() {return G::label()+ "/" + _full(); } template - static StringType _label() {return G::label(); } + static StringType _full() {return G::label(); } + + + static StringType messageNameLast() {return _last();} + + template + static StringType _last() {return _last(); } + template + static StringType _last() {return G::label(); } }; //! @@ -76,14 +84,27 @@ struct JSKRecognitionMsgs { static StringType label() {return "jsk_recognition_msgs";} //! - //! \brief The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message. + //! \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 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. + //! + //! The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message. + struct ProgressGroup { + static StringType label() {return "Progress";} + }; +}; + typedef MessageGroup @@ -109,6 +130,10 @@ typedef MessageGroups::MessageGroup PolygonArrayGroup; +typedef MessageGroups::MessageGroup + ProgressGroup; + } // end namespace MessageGroups } // end namespace ros_bridge diff --git a/src/comm/ros_bridge/include/PackageBuffer.h b/src/comm/ros_bridge/include/PackageBuffer.h new file mode 100644 index 0000000000000000000000000000000000000000..27ea3d03a841048d5e2781673289f3f04267efa9 --- /dev/null +++ b/src/comm/ros_bridge/include/PackageBuffer.h @@ -0,0 +1,47 @@ +#pragma once +#pragma once + +#include "boost/lockfree/queue.hpp" +#include + +namespace ROSBridge { +namespace Bridge { +namespace lf = ::boost::lockfree; +template +class PackageBuffer +{ +public: + PackageBuffer(); + + void push(T t) { + buffer.push(t); + if (_pushCallback) + _pushCallback(); + } + + T pop() { + T temp = buffer.pop(); + if (_popCallback) + _popCallback(); + return temp; + } + + void setPushCallback(std::function pushCallback) { + _pushCallback = pushCallback; + } + + void setPopCallback(std::function popCallback) { + _popCallback = popCallback; + } + + bool empty() const { return buffer.empty();} +private: + lf::queue buffer; + std::function _popCallback; + std::function _pushCallback; +}; + +} // namespace Communicator + +} // namespace + diff --git a/src/comm/ros_bridge/include/ROSCommunicator.h b/src/comm/ros_bridge/include/ROSCommunicator.h index 6bf821382fb1ee7538216101d7a610a26716cbf2..61a35e96e7c94b980b35d80671662b0247deb2c6 100644 --- a/src/comm/ros_bridge/include/ROSCommunicator.h +++ b/src/comm/ros_bridge/include/ROSCommunicator.h @@ -1,17 +1,29 @@ #pragma once -#include "ros_bridge/include/MessageBaseClass.h" +#include "ros_bridge/rapidjson/include/rapidjson/document.h" -typedef ROSBridge::MessageBaseClass ROSMsg; +#include +#include -class ROSCommunicator +#include "boost/lockfree/queue.hpp" +#include "ros_bridge/include/ + +namespace ROSBridge { +namespace lf = ::boost::lockfree; +class Communicator { + typedef std::unique_ptr UniqueJsonPtr; + typedef std::tuple MsgTopicHashPair; public: - explicit ROSCommunicator() {} - - void send(const ROSMsg *msg); + explicit Communicator() {} + void send(UniqueJsonPtr &msg); + void start(); + void stop(); + virtual UniqueJsonPtr receive() = 0; -public : - virtual void receive(ROSMsg *msg) = 0; +private: + lf::queue _transmittBuffer; + lf::queue _receiveBuffer; }; +} diff --git a/src/comm/ros_bridge/include/TypeFactory.h b/src/comm/ros_bridge/include/TypeFactory.h index edb6916407e32316c8804d63111d3ef4db3bbc74..b02ac13ee64f545ab86937b5e3e9b65b5dcd9c91 100644 --- a/src/comm/ros_bridge/include/TypeFactory.h +++ b/src/comm/ros_bridge/include/TypeFactory.h @@ -1,5 +1,4 @@ #pragma once -#pragma once #include "ros_bridge/rapidjson/include/rapidjson/document.h" #include "ros_bridge/include/JsonMethodes.h" @@ -19,10 +18,9 @@ namespace ROSBridge { //! //! The TypeFactory class is used to create C++ representatives of ROS messages //! (classes derived from \class MessageBaseClass) from a \class rapidjson::Document document. -template class TypeFactory { - typedef MessageBaseClass ROSMsg; + typedef MessageBaseClass ROSMsg; public: TypeFactory(){} @@ -61,7 +59,7 @@ private: template bool _create(const rapidjson::Document &doc, U &type, Type2Type){ using namespace ROSBridge; - bool ret = JsonMethodes::Point32::fromJson(doc, type); + bool ret = JsonMethodes::GeometryMsgs::Point32::fromJson(doc, type); assert(ret); return ret; } @@ -72,7 +70,7 @@ private: template bool _create(const rapidjson::Document &doc, U &type, Type2Type){ using namespace ROSBridge; - bool ret = JsonMethodes::GeoPoint::fromJson(doc, type); + bool ret = JsonMethodes::GeographicMsgs::GeoPoint::fromJson(doc, type); assert(ret); return ret; } @@ -83,7 +81,7 @@ private: template bool _create(const rapidjson::Document &doc, U &type, Type2Type){ using namespace ROSBridge; - bool ret = JsonMethodes::Polygon::fromJson(doc, type); + bool ret = JsonMethodes::GeometryMsgs::Polygon::fromJson(doc, type); assert(ret); return ret; } @@ -94,7 +92,7 @@ private: template bool _create(const rapidjson::Document &doc, U &type, Type2Type){ using namespace ROSBridge; - bool ret = JsonMethodes::PolygonStamped::fromJson(doc, type); + bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::fromJson(doc, type); assert(ret); return ret; } @@ -105,7 +103,18 @@ private: template bool _create(const rapidjson::Document &doc, U &type, Type2Type){ using namespace ROSBridge; - bool ret = JsonMethodes::PolygonArray::fromJson(doc, type); + 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; } diff --git a/src/comm/ros_bridge/src/CasePacker.cpp b/src/comm/ros_bridge/src/CasePacker.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f75046ca39610472be4342190efdb2d50326bda4 --- /dev/null +++ b/src/comm/ros_bridge/src/CasePacker.cpp @@ -0,0 +1,39 @@ +#include "CasePacker.h" + +const char* ROSBridge::CasePacker::topicKey = "topic"; +const char* ROSBridge::CasePacker::messageTypeKey = "messageType"; + +ROSBridge::CasePacker::CasePacker() +{ + +} + +void ROSBridge::CasePacker::_addTag(JsonDoc &doc, const char *topic, const char *messageType) +{ + using namespace ROSBridge; + using namespace rapidjson; + + { + // add topic + rapidjson::Value key(CasePacker::topicKey, doc.GetAllocator()); + rapidjson::Value value(topic, doc.GetAllocator()); + doc.AddMember(key, value, doc.GetAllocator()); + } + + // add messageType + rapidjson::Value key(CasePacker::messageTypeKey, doc.GetAllocator()); + rapidjson::Value value(messageType, doc.GetAllocator()); + doc.AddMember(key, value, doc.GetAllocator()); +} + +void ROSBridge::CasePacker::_removeTag(JsonDoc &doc) +{ + using namespace ROSBridge; + using namespace rapidjson; + if ( doc.HasMember(CasePacker::topicKey) ) + doc.RemoveMember(CasePacker::topicKey); + if ( doc.HasMember(CasePacker::messageTypeKey) ) + doc.RemoveMember(CasePacker::messageTypeKey); +} + + diff --git a/src/comm/ros_bridge/src/CasePacker.h b/src/comm/ros_bridge/src/CasePacker.h new file mode 100644 index 0000000000000000000000000000000000000000..7f959db5881469c41d75caca3077e786cb13eda0 --- /dev/null +++ b/src/comm/ros_bridge/src/CasePacker.h @@ -0,0 +1,42 @@ +#pragma once +#include "ros_bridge/include/MessageBaseClass.h" + +#include +#include "rapidjson/include/rapidjson/document.h" + +namespace ROSBridge { + +class CasePacker +{ + typedef rapidjson::Document JsonDoc; + typedef std::unique_ptr UniqueJsonPtr; +public: + CasePacker(); + + struct MessageTag { + char *topic; + char *messagType; + }; + typedef MessageTag Tag; + + + template + void packAndSend(const T &msg, const char *topic); + + const Tag &showTag(); + + template + void unpack(T &msg); + +protected: + void _addTag(JsonDoc &doc, const char *topic, const char *messageType); + void _removeTag(JsonDoc &doc); + + static const char* topicKey; + static const char* messageTypeKey; + +private: + Tag _tag; +}; +} + diff --git a/src/comm/ros_bridge/src/PackageBuffer.h b/src/comm/ros_bridge/src/PackageBuffer.h new file mode 100644 index 0000000000000000000000000000000000000000..c41163e23d5c7b18844945c9ccae2fea467da5d5 --- /dev/null +++ b/src/comm/ros_bridge/src/PackageBuffer.h @@ -0,0 +1,45 @@ +#pragma once + +#include "boost/lockfree/queue.hpp" +#include + +namespace ROSBridge { +namespace Bridge { +namespace lf = ::boost::lockfree; +template +class PackageBuffer +{ +public: + PackageBuffer(); + + void push(T t) { + buffer.push(t); + if (_pushCallback) + _pushCallback(); + } + + T pop() { + T temp = buffer.pop(); + if (_popCallback) + _popCallback(); + return temp; + } + + void setPushCallback(std::function pushCallback) { + _pushCallback = pushCallback; + } + + void setPopCallback(std::function popCallback) { + _popCallback = popCallback; + } + + bool empty() const { return buffer.empty();} +private: + lf::queue buffer; + std::function _popCallback; + std::function _pushCallback; +}; + +} // namespace Communicator + +} // namespace diff --git a/src/comm/ros_bridge/src/ROSCommunicator.cpp b/src/comm/ros_bridge/src/ROSCommunicator.cpp index fbd134ee58fdeee4293ae46b146d7fb00194a513..2f335da25f94f810a02594915cfcd32cd8869d6e 100644 --- a/src/comm/ros_bridge/src/ROSCommunicator.cpp +++ b/src/comm/ros_bridge/src/ROSCommunicator.cpp @@ -1 +1,6 @@ #include "ros_bridge/include/ROSCommunicator.h" + +void ROSBridge::Communicator::send(ROSBridge::Communicator::UniqueJsonPtr &msg) +{ + if (!msg->HasMember("")) +}