From 652511a1520820125eb11886a49dad845c859c40 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Sat, 4 Jul 2020 11:43:04 +0200 Subject: [PATCH] Testing ros_bridge/include/messages.h --- src/Wima/WimaController.cc | 96 ++++++++++++-- src/comm/ros_bridge/include/jsongenerator.h | 133 -------------------- src/comm/ros_bridge/include/messages.h | 57 +++++---- 3 files changed, 120 insertions(+), 166 deletions(-) diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index cfc3d395a..47ed4d10f 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -1,6 +1,6 @@ #include "WimaController.h" #include "utilities.h" -#include "ros_bridge/include/jsongenerator.h" +#include "ros_bridge/include/messages.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" @@ -8,6 +8,8 @@ #include "time.h" #include "assert.h" +#include "QVector3D" + const char* WimaController::wimaFileExtension = "wima"; const char* WimaController::areaItemsName = "AreaItems"; @@ -961,7 +963,7 @@ void WimaController::_eventTimerHandler() { static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL); static EventTicker snakeEventLoopTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL); - static EventTicker ros_bridgeTicker(EVENT_TIMER_INTERVAL, 1000); + static EventTicker rosBridgeTicker(EVENT_TIMER_INTERVAL, 1000); // Battery level check necessary? if ( batteryLevelTicker.ready() ) @@ -993,20 +995,98 @@ void WimaController::_eventTimerHandler() } } - if (ros_bridgeTicker.ready()) { + if (rosBridgeTicker.ready()) { using namespace ros_bridge; + + // Time class Time time(1, 2); - JsonGenerator<> gen; - gen.set(time); rapidjson::Document doc(rapidjson::kObjectType); - // Write to stdout - cout << "Return value : " << gen.run(MessageType::Time , doc, doc.GetAllocator()) << std::endl; + cout << "Time" << endl; + cout << "Return value : " << time.toJson(doc, doc.GetAllocator()) << std::endl; rapidjson::OStreamWrapper out(std::cout); rapidjson::Writer writer(out); doc.Accept(writer); - std::cout << std::endl; + std::cout << std::endl << std::endl; + + + // Header + Header header(1, &time, "/map"); + // Write to stdout + cout << "Header" << endl; + rapidjson::Document doc2(rapidjson::kObjectType); + cout << "Return value : " << header.toJson(doc2, doc2.GetAllocator()) << std::endl; + rapidjson::Writer writer2(out); + doc2.Accept(writer2); + std::cout << std::endl << std::endl; + + + // Point + QVector3D p1(1, 2, 3); + typedef Point32 PointWrapper; + PointWrapper pw1(&p1); + // Write to stdout + cout << "Point" << endl; + rapidjson::Document doc3(rapidjson::kObjectType); + cout << "Return value : " << pw1.toJson(doc3, doc3.GetAllocator()) << std::endl; + rapidjson::Writer writer3(out); + doc3.Accept(writer3); + std::cout << std::endl << std::endl; + + + // Polygon + QVector3D p2(4, 5, 6); + QVector3D p3(7, 8, 9); + PointWrapper pw2(&p2); + PointWrapper pw3(&p3); + QVector pointList; + pointList.append(pw1); + pointList.append(pw2); + pointList.append(pw3); + typedef Polygon Poly; + Poly polyW(&pointList); + // Write to stdout + cout << "Polygon" << endl; + rapidjson::Document doc4(rapidjson::kObjectType); + cout << "Return value : " << polyW.toJson(doc4, doc4.GetAllocator()) << std::endl; + rapidjson::Writer writer4(out); + doc4.Accept(writer4); + std::cout << std::endl << std::endl; + + + // PolygonStamped + typedef PolygonStamped PolyWrapper; + PolyWrapper polyStamped(&header, &polyW); + // Write to stdout + cout << "PolygonStamped" << endl; + rapidjson::Document doc5(rapidjson::kObjectType); + cout << "Return value : " << polyStamped.toJson(doc5, doc5.GetAllocator()) << std::endl; + rapidjson::Writer writer5(out); + doc5.Accept(writer5); + std::cout << std::endl << std::endl; + + + // PolygonArray + // Define second Polygon + QVector3D p4(10, 11, 12); + QVector3D p5(13, 14, 15); + QVector3D p6(16, 17, 18); + PointWrapper pw4(&p4); + PointWrapper pw5(&p5); + PointWrapper pw6(&p6); + QVector pointList2{pw4, pw5, pw6}; + Poly polyW2(&pointList2); + typedef PolygonArray PolyArray; + QVector polygons{&polyW, &polyW2}; + PolyArray polyArray{}; + // Write to stdout + cout << "PolygonStamped" << endl; + rapidjson::Document doc5(rapidjson::kObjectType); + cout << "Return value : " << polyStamped.toJson(doc5, doc5.GetAllocator()) << std::endl; + rapidjson::Writer writer5(out); + doc5.Accept(writer5); + std::cout << std::endl << std::endl; } } diff --git a/src/comm/ros_bridge/include/jsongenerator.h b/src/comm/ros_bridge/include/jsongenerator.h index f006ad0fd..76dfa420a 100644 --- a/src/comm/ros_bridge/include/jsongenerator.h +++ b/src/comm/ros_bridge/include/jsongenerator.h @@ -1,138 +1,5 @@ #ifndef JSONGENERATOR_H #define JSONGENERATOR_H -#include "rapidjson/include/rapidjson/document.h" -#include "include/messages.h" -#include "assert.h" -namespace ros_bridge { - -enum MessageType{TimeMessage, - HeaderMessage, - Point32Message, - PolygonMessage, - PolygonStampedMessage, - PolygonArrayMessage}; - -template , - class PolygonStampedType = class PolygonStamped<> , - class PolygonArrayType = class PolygonArray<> > -class JsonGenerator { -public: - - JsonGenerator() : _time(nullptr) {} - - // Add set() function if new MessageType required! - - void add(const TimeType &time) {_time = &time;} - void add(const HeaderType &header) {_header = &header;} - void add(const PointType &point) {_point = &point;} - void add(const PolygonType &polygon) {_polygon = &polygon;} - - - // Modify run() function if new MessageType required! - - bool run(MessageType type, JsonDocumentType &doc, AllocatorType &allocator) { - switch (type) { - case TimeMessage: - if (_time != nullptr) - return _timeToJson(doc, allocator); - assert(false); // _time not set! - break; - - case HeaderMessage: - if (_header != nullptr) - return _headerToJson(doc, allocator); - assert(false); // _header not set! - break; - - case Point32Message: - if (_point != nullptr) - return _point32ToJson(doc, allocator); - assert(false); // _point not set! - break; - - case PolygonMessage: - if (_polygon != nullptr) - return _polygonToJson(doc, allocator); - assert(false); // _polygon not set! - break; - - case PolygonStampedMessage: - if (_polygonStamped != nullptr) - return _polygonStampedToJson(doc, allocator); - assert(false); // _polygonStamped not set! - break; - - case PolygonArrayMessage: - if (_polygonArray != nullptr) - return _polygonArrayToJson(doc, allocator); - assert(false); // _polygonArray not set! - break; - default: - assert(false); // Unknown MessageType! - } - return false; - } - - - // Modify clear() function if new MessageType required! - - void clear() { - _time = nullptr; - _header = nullptr; - _point = nullptr; - _polygon = nullptr; - } - -private: - bool _timeToJson(JsonDocumentType &doc, AllocatorType &allocator) { - doc.AddMember("secs", rapidjson::Value().SetUint(_time->getSecs()), allocator); - doc.AddMember("nsecs", rapidjson::Value().SetUint(_time->getNSecs()), allocator); - - return true; - } - - // TODO - bool _HeaderToJson(JsonDocumentType &doc, AllocatorType &allocator) { - return false; - } - - bool _Point32ToJson(JsonDocumentType &doc, AllocatorType &allocator) { - return false; - } - - bool _PolygonToJson(JsonDocumentType &doc, AllocatorType &allocator) { - return false; - } - - bool _PolygonStampedToJson(JsonDocumentType &doc, AllocatorType &allocator) { - return false; - } - - bool _PolygonArrayToJson(JsonDocumentType &doc, AllocatorType &allocator) { - return false; - } - - - - // Add new MessageType here. - const TimeType *_time; - const HeaderType *_header; - const PointType *_point; - const PolygonType *_polygon; - const PolygonStampedType *_polygonStamped; - const PolygonArrayType *_polygonArray; -}; - -namespace detail { -} - -} #endif // JSONGENERATOR_H diff --git a/src/comm/ros_bridge/include/messages.h b/src/comm/ros_bridge/include/messages.h index e666dfa4b..ab940493a 100644 --- a/src/comm/ros_bridge/include/messages.h +++ b/src/comm/ros_bridge/include/messages.h @@ -3,7 +3,6 @@ #include #include -#include #include "ros_bridge/rapidjson/include/rapidjson/rapidjson.h" #include "ros_bridge/rapidjson/include/rapidjson/document.h" @@ -17,13 +16,16 @@ namespace ros_bridge { Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {} uint32_t getSecs() const {return _secs;} - uint32_t getNSecs() const {return _secs;} + uint32_t getNSecs() const {return _nsecs;} + + void setSecs(uint32_t secs) {_secs = secs;} + void setNSecs(uint32_t nsecs) {_nsecs = nsecs;} bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) const { - doc.AddMember("secs", rapidjson::Value().SetUint(this->getSecs()), allocator); - doc.AddMember("nsecs", rapidjson::Value().SetUint(this->getNSecs()), allocator); + doc.AddMember("secs", rapidjson::Value().SetUint(this->_secs), allocator); + doc.AddMember("nsecs", rapidjson::Value().SetUint(this->_nsecs), allocator); return true; } @@ -46,9 +48,9 @@ namespace ros_bridge { const Time *getStamp() const {return _stamp;}; const std::string &getFrameId() const {return _frameId;}; - void setSeq (uint32_t seq); - void setStamp (const Time *stamp); - void setFrameId (const std::string &frameId); + void setSeq (uint32_t seq) {_seq = seq;} + void setStamp (const Time *stamp) {_stamp = stamp;} + void setFrameId (const std::string &frameId) {_frameId = frameId;} bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) const @@ -77,7 +79,7 @@ namespace ros_bridge { template class Point32{ public: - Point32(): Point32(0, 0, 0) {} + Point32() {} Point32(const PointType* p): _point(p) {} const PointType *getPoint() const {return _point;} @@ -99,12 +101,12 @@ namespace ros_bridge { //! @brief C++ representation of geometry_msgs/Polygon template , - template class ContainerType = std::vector> + template class ContainerType = std::vector, + class PointWrapper = class Point32 > class Polygon{ public: Polygon(){} - Polygon(const ContainerType *points) : _points(&points) {} + Polygon(const ContainerType *points) : _points(points) {} const ContainerType *getPoints() const {return _points;} @@ -117,7 +119,7 @@ namespace ros_bridge { for(long i=0; i < _points->size(); ++i) { rapidjson::Document point(rapidjson::kObjectType); - if (_points[i].toJson(point, allocator)) + if ( !( (*_points)[i].toJson(point, allocator) ) ) return false; points.PushBack(point, allocator); } @@ -132,7 +134,7 @@ namespace ros_bridge { }; //! @brief C++ representation of geometry_msgs/PolygonStamped - template class PolygonType = Polygon> + template , class HeaderType = Header> class PolygonStamped : public PolygonType{ public: PolygonStamped(); @@ -144,7 +146,7 @@ namespace ros_bridge { const HeaderType *getHeader() const {return _header;} const PolygonType *getPolygon() const {return _polygon;} - void setHeader(const Header *header) {_header = header;} + void setHeader(const HeaderType *header) {_header = header;} void setPolygon(const PolygonType *polygon) {_polygon = polygon;} bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) @@ -168,13 +170,13 @@ namespace ros_bridge { //! @brief C++ representation of jsk_recognition_msgs/PolygonArray template , - template class ContainerType = std::vector > + template class ContainerType = std::vector, + class HeaderType = Header > class PolygonArray{ public: PolygonArray() {} - PolygonArray(const HeaderType *header, + PolygonArray(const HeaderType *header, const ContainerType *polygons, const ContainerType *labels, const ContainerType<_Float32> *likelihood) @@ -184,18 +186,23 @@ namespace ros_bridge { , _likelihood(likelihood) {} - const HeaderType &getHeader() const {return _header;} - const std::vector &getPolygons() const {return _polygons;} - const std::vector &getLabels() const {return _labels;} - const std::vector<_Float32> &getLikelihood() const {return _likelihood;} + const HeaderType *getHeader() const {return _header;} + const ContainerType *getPolygons() const {return _polygons;} + const ContainerType *getLabels() const {return _labels;} + const ContainerType<_Float32> *getLikelihood() const {return _likelihood;} + + void *setHeader (const HeaderType *header) {_header = header;} + void *setPolygons (const ContainerType *polygon) {_polygons = polygon;} + void *setLabels (const ContainerType *labels) {_labels = labels;} + void *setLikelihood (const ContainerType<_Float32> *likelihood) {_likelihood = likelihood;} private: - HeaderType _header; - std::vector _polygons; - std::vector _labels; - std::vector<_Float32> _likelihood; + HeaderType *_header; + ContainerType *_polygons; + ContainerType *_labels; + ContainerType<_Float32> *_likelihood; }; } -- 2.22.0