Testing ros_bridge/include/messages.h

src/Wima/WimaController.cc

 #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<rapidjson::OStreamWrapper> 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<rapidjson::OStreamWrapper> writer2(out);
+        doc2.Accept(writer2);
+        std::cout << std::endl << std::endl;
+
+
+        // Point
+        QVector3D p1(1, 2, 3);
+        typedef Point32<QVector3D> 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<rapidjson::OStreamWrapper> 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<PointWrapper> pointList;
+        pointList.append(pw1);
+        pointList.append(pw2);
+        pointList.append(pw3);
+        typedef Polygon<PointWrapper, QVector> 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<rapidjson::OStreamWrapper> writer4(out);
+        doc4.Accept(writer4);
+        std::cout << std::endl << std::endl;
+
+
+        // PolygonStamped
+        typedef PolygonStamped<PointWrapper, Poly> 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<rapidjson::OStreamWrapper> 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<PointWrapper> pointList2{pw4, pw5, pw6};
+        Poly polyW2(&pointList2);
+        typedef PolygonArray<PointWrapper, Poly, QVector> PolyArray;
+        QVector<Poly*> 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<rapidjson::OStreamWrapper> writer5(out);
+        doc5.Accept(writer5);
+        std::cout << std::endl << std::endl;
     }
 }
 

src/comm/ros_bridge/include/jsongenerator.h

 #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 JsonDocumentType = rapidjson::Document,
-          class AllocatorType    = rapidjson::Document::AllocatorType,
-
-          class TimeType            = class Time ,
-          class HeaderType          = class Header ,
-          class PointType           = class Point32 ,
-          class PolygonType         = class Polygon<> ,
-          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

src/comm/ros_bridge/include/messages.h

@@ -3,7 +3,6 @@
 
 #include <iostream>
 #include <vector>
-#include <array>
 
 #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 PointType>
     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 <class PointType,
-              class PointWrapper  = class Point32<PointType>,
-              template <class> class ContainerType = std::vector>
+              template <class, class...> class ContainerType =  std::vector,
+              class PointWrapper  = class Point32<PointType> >
     class Polygon{
     public:
         Polygon(){}
-        Polygon(const ContainerType<PointType> *points) : _points(&points) {}
+        Polygon(const ContainerType<PointType> *points) : _points(points) {}
 
 
         const ContainerType<PointType> *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 PointType, class HeaderType = Header, template <class> class PolygonType = Polygon>
+    template <class PointType, class PolygonType = Polygon<PointType>, 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 <class PointType,
-              class HeaderType = Header,
               class PolygonType = Polygon<PointType>,
-              template <class> class ContainerType = std::vector >
+              template <class, class...> class ContainerType = std::vector,
+              class HeaderType = Header >
     class PolygonArray{
     public:
         PolygonArray() {}
-        PolygonArray(const HeaderType                 *header,
+        PolygonArray(const HeaderType                   *header,
                      const ContainerType<PolygonType>   *polygons,
                      const ContainerType<uint32_t>      *labels,
                      const ContainerType<_Float32>      *likelihood)
@@ -184,18 +186,23 @@ namespace ros_bridge {
           , _likelihood(likelihood)
         {}
 
-        const HeaderType               &getHeader()     const {return _header;}
-        const std::vector<PolygonType> &getPolygons()   const {return _polygons;}
-        const std::vector<uint32_t>    &getLabels()     const {return _labels;}
-        const std::vector<_Float32>    &getLikelihood() const {return _likelihood;}
+        const HeaderType                 *getHeader()     const {return _header;}
+        const ContainerType<PolygonType> *getPolygons()   const {return _polygons;}
+        const ContainerType<uint32_t>    *getLabels()     const {return _labels;}
+        const ContainerType<_Float32>    *getLikelihood() const {return _likelihood;}
+
+        void *setHeader     (const HeaderType                 *header)      {_header     = header;}
+        void *setPolygons   (const ContainerType<PolygonType> *polygon)     {_polygons   = polygon;}
+        void *setLabels     (const ContainerType<uint32_t>    *labels)      {_labels     = labels;}
+        void *setLikelihood (const ContainerType<_Float32>    *likelihood)  {_likelihood = likelihood;}
 
 
 
     private:
-        HeaderType                _header;
-        std::vector<PolygonType>  _polygons;
-        std::vector<uint32_t>     _labels;
-        std::vector<_Float32>     _likelihood;
+        HeaderType                 *_header;
+        ContainerType<PolygonType> *_polygons;
+        ContainerType<uint32_t>    *_labels;
+        ContainerType<_Float32>    *_likelihood;
     };
 }