working on ros_bridge, code not compilable.

src/Wima/WimaController.cc

 #include "WimaController.h"
 #include "utilities.h"
+#include "ros_bridge/include/jsongenerator.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 "time.h"
 #include "assert.h"
@@ -957,6 +961,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);
 
     // Battery level check necessary?
     if ( batteryLevelTicker.ready() )
@@ -987,6 +992,22 @@ void WimaController::_eventTimerHandler()
             emit snakeProgressChanged();
         }
     }
+
+    if (ros_bridgeTicker.ready()) {
+        using namespace ros_bridge;
+
+        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;
+        rapidjson::OStreamWrapper out(std::cout);
+        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
+        doc.Accept(writer);
+        std::cout << std::endl;
+    }
 }
 
 void WimaController::_smartRTLCleanUp(bool flying)

src/comm/ros_bridge/include/jsongenerator.h

@@ -2,15 +2,137 @@
 #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 TimeType,
-          >
+          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

@@ -5,19 +5,28 @@
 #include <vector>
 #include <array>
 
+#include "ros_bridge/rapidjson/include/rapidjson/rapidjson.h"
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+
 namespace ros_bridge {
 
     //! @brief C++ representation of ros::Time
     class Time{
     public:
-        Time();
-        Time(uint32_t secs, uint32_t nsecs);
+        Time(): _secs(0), _nsecs(0) {}
+        Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {}
 
-        uint32_t getSecs()  const;
-        uint32_t getNSecs() const;
+        uint32_t getSecs()  const {return _secs;}
+        uint32_t getNSecs() const {return _secs;}
 
-        void setSecs    (uint32_t secs);
-        void setNSecs   (uint32_t 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);
+
+            return true;
+        }
 
     private:
         uint32_t _secs;
@@ -27,128 +36,166 @@ namespace ros_bridge {
     //! @brief C++ representation of std_msgs/Header
     class Header{
     public:
-        Header();
-        Header(uint32_t seq, const Time &stamp, const std::string &frame_id);
+        Header() : _seq(0), _frameId("") {}
+        Header(uint32_t seq, const Time *stamp, const std::string &frame_id) :
+            _seq(seq)
+          , _stamp(stamp)
+          , _frameId(frame_id) {}
 
-        uint32_t            getSeq()       const;
-        const Time         &getStamp()     const;
-        const std::string  &getFrameId()   const;
+        uint32_t            getSeq()       const {return _seq;};
+        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  &frame_id);
+        void setStamp   (const Time *stamp);
+        void setFrameId (const std::string  &frameId);
+
+        bool toJson(rapidjson::Document &doc,
+                    rapidjson::Document::AllocatorType &allocator) const
+        {
+            doc.AddMember("seq", rapidjson::Value().SetUint(this->_seq), allocator);
+            rapidjson::Document stamp(rapidjson::kObjectType);
+            if (!this->_stamp->toJson(stamp, allocator))
+                    return false;
+            doc.AddMember("stamp", stamp, allocator);
+            doc.AddMember("frame_id",
+                          rapidjson::Value().SetString(this->_frameId.data(),
+                                                       this->_frameId.length(),
+                                                       allocator),
+                          allocator);
+
+            return true;
+        }
 
     private:
         uint32_t    _seq;
-        Time        _stamp;
-        std::string _frame_id;
+        const Time *_stamp;
+        std::string _frameId;
     };
 
     //! @brief C++ representation of geometry_msgs/Point32
+    template <class PointType>
     class Point32{
-    public:
-        Point32();
-        Point32(_Float32 x, _Float32 y, _Float32 z);
+    public:        
+        Point32(): Point32(0, 0, 0) {}
+        Point32(const PointType* p): _point(p) {}
 
-        template<unsigned int i>
-        void set(_Float32 component)
-        {
-            static_assert (i < 3, "Index out of bonds.");
-            _components[i] = component;
-        }
+        const PointType    *getPoint()                          const   {return _point;}
+        void                setPoint(const PointType *point)            {point;}
 
-        template<unsigned int i>
-        _Float32 get(void) const
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) const
         {
-            static_assert (i < 3, "Index out of bonds.");
-            return _components[i];
+
+            doc.AddMember("x", rapidjson::Value().SetFloat(_point->x()), allocator);
+            doc.AddMember("y", rapidjson::Value().SetFloat(_point->y()), allocator);
+            doc.AddMember("z", rapidjson::Value().SetFloat(_point->z()), allocator);
+
+            return true;
         }
 
     private:
-        std::array<_Float32, 3> _components;
+        const PointType *_point;
     };
 
     //! @brief C++ representation of geometry_msgs/Polygon
-    template <class PointType = Point32>
+    template <class PointType,
+              class PointWrapper  = class Point32<PointType>,
+              template <class> class ContainerType = std::vector>
     class Polygon{
     public:
         Polygon(){}
-        Polygon(const std::vector<Point32> &points) : _points(points) {}
-        Polygon(const Polygon &poly) : _points(poly.get()) {}
+        Polygon(const ContainerType<PointType> *points) : _points(&points) {}
 
-        const std::vector<Point32> &get() const
-        {
-            return _points;
-        }
 
-        void set(std::vector<Point32> &points)
-        {
-            _points = points;
-        }
+        const ContainerType<PointType> *getPoints() const {return _points;}
 
-        void clear()
-        {
-            _points.clear();
-        }
+        void setPoints(const ContainerType<PointType> *points) {_points = points;}
 
-        PointType &at(unsigned int i)
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
         {
-            return _points.at(i);
-        }
+            rapidjson::Value points(rapidjson::kArrayType);
 
-        void push_back(const Point32 &p)
-        {
-            _points.push_back(p);
-        }
+            for(long i=0; i < _points->size(); ++i) {
+                rapidjson::Document point(rapidjson::kObjectType);
+                if (_points[i].toJson(point, allocator))
+                    return false;
+                points.PushBack(point, allocator);
+            }
 
-        void pop_back()
-        {
-            _points.pop_back();
+            doc.AddMember("points", points, allocator);
+
+            return true;
         }
 
     private:
-        std::vector<PointType> _points;
+        const ContainerType<PointType> *_points;
     };
 
     //! @brief C++ representation of geometry_msgs/PolygonStamped
-    template <class HeaderType = Header, class PolygonType = Polygon<>>
+    template <class PointType, class HeaderType = Header, template <class> class PolygonType = Polygon>
     class PolygonStamped : public PolygonType{
     public:
         PolygonStamped();
-        PolygonStamped(const Header &header, const PolygonType &polygon) :
+        PolygonStamped(const Header *header, const PolygonType *polygon) :
             _header(header)
-          , PolygonType(polygon) {}
+          , _polygon(polygon) {}
+
+
+        const HeaderType  *getHeader()  const   {return _header;}
+        const PolygonType *getPolygon() const   {return _polygon;}
+
+        void setHeader(const Header *header)          {_header = header;}
+        void setPolygon(const PolygonType *polygon)   {_polygon = polygon;}
+
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
+        {
+            rapidjson::Document header(rapidjson::kObjectType);
+            if (!this->_header->toJson(header, allocator))
+                return false;
+            rapidjson::Document polygon(rapidjson::kObjectType);
+            if (!this->_polygon->toJson(polygon, allocator))
+                return false;
+            doc.AddMember("header", header, allocator);
+            doc.AddMember("polygon", polygon, allocator);
+
+            return true;
+        }
 
     private:
-        Header _header;
+        HeaderType *_header;
+        PolygonType *_polygon;
     };
 
     //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
-    template <class PolygonType = Polygon<>, class HeaderType = Header, template<class> class ContainerType = std::vector >
+    template <class PointType,
+              class HeaderType = Header,
+              class PolygonType = Polygon<PointType>,
+              template <class> class ContainerType = std::vector >
     class PolygonArray{
     public:
-        PolygonArray()
-         : PolygonArray(HeaderType()
-                        , ContainerType<PolygonType>()
-                        , ContainerType<uint32_t>()
-                        , ContainerType<_Float32>())
-        {}
-
-        PolygonArray(const HeaderType                   &header,
-                     const ContainerType<PolygonType>   &polygons,
-                     const ContainerType<uint32_t>      &labels,
-                     const ContainerType<_Float32>      &likelihood)
+        PolygonArray() {}
+        PolygonArray(const HeaderType                 *header,
+                     const ContainerType<PolygonType>   *polygons,
+                     const ContainerType<uint32_t>      *labels,
+                     const ContainerType<_Float32>      *likelihood)
           : _header(header)
           , _polygons(polygons)
           , _labels(labels)
           , _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;}
+
+
+
     private:
-        HeaderType                  _header;
-        ContainerType<PolygonType>  _polygons;
-        ContainerType<uint32_t>     _labels;
-        ContainerType<_Float32>     _likelihood;
+        HeaderType                _header;
+        std::vector<PolygonType>  _polygons;
+        std::vector<uint32_t>     _labels;
+        std::vector<_Float32>     _likelihood;
     };
 }
 

src/comm/ros_bridge/src/messages.cpp

-#include "include/messages.h"
-
-using namespace ros_bridge;
-
-//===================================================================
-// Time
-//===================================================================
-
-Time::Time(): _secs(0), _nsecs(0) {}
-
-Time::Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {}
-
-uint32_t Time::getSecs() const
-{
-    return _secs;
-}
-
-uint32_t Time::getNSecs() const
-{
-    return _nsecs;
-}
-
-void Time::setSecs(uint32_t secs)
-{
-    _secs = secs;
-}
-
-void Time::setNSecs(uint32_t nsecs)
-{
-    _nsecs = nsecs;
-}
-
-
-//===================================================================
-// Header
-//===================================================================
-Header::Header():  _seq(0), _frame_id("") {}
-
-Header::Header(uint32_t seq, const Time &stamp, const std::string &frame_id):  _seq(seq), _stamp(stamp), _frame_id(frame_id) {}
-
-uint32_t Header::getSeq() const
-{
-    return _seq;
-}
-
-const Time &Header::getStamp() const
-{
-    return _stamp;
-}
-
-const std::string &Header::getFrameId() const
-{
-    return _frame_id;
-}
-
-void Header::setSeq(uint32_t seq)
-{
-    _seq = seq;
-}
-
-void Header::setStamp(const Time &stamp)
-{
-    _stamp = stamp;
-}
-
-void Header::setFrameId(const std::string &frame_id)
-{
-    _frame_id = frame_id;
-}
-
-//===================================================================
-// Point32
-//===================================================================
-Point32::Point32(): Point32(0, 0, 0) {}
-
-Point32::Point32(_Float32 x, _Float32 y, _Float32 z): _components{x, y, z} {}
-
-
-//===================================================================
-// PolygonStamped
-//===================================================================
-PolygonStamped::PolygonStamped() {}
-
-PolygonStamped::PolygonStamped(const Header &header, const Polygon &polygon) : header(header), polygon(polygon){}
-
-bool PolygonStamped::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
-{
-    rapidjson::Document header(rapidjson::kObjectType);
-    if (!this->header.toJson(header, allocator))
-        return false;
-    rapidjson::Document polygon(rapidjson::kObjectType);
-    if (!this->polygon.toJson(polygon, allocator))
-        return false;
-    doc.AddMember("header", header, allocator);
-    doc.AddMember("polygon", polygon, allocator);
-
-    return true;
-}
-
-
-
-//===================================================================
-// PolygonArray
-//===================================================================
-PolygonArray::PolygonArray() {}
-
-PolygonArray::PolygonArray(const Header &header,
-                           const std::vector<PolygonStamped> &polygons,
-                           const std::vector<uint32_t> &labels,
-                           const std::vector<_Float32> &likelihood)
-    : header(header), polygons(polygons), labels(labels), likelihood(likelihood) {}
-
-bool PolygonArray::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
-{
-    rapidjson::Document header(rapidjson::kObjectType);
-    if (!this->header.toJson(header, allocator))
-        return false;
-    doc.AddMember("header", header, allocator);
-
-    rapidjson::Value polygons(rapidjson::kArrayType);
-
-    for(auto it = this->polygons.begin(); it != this->polygons.end(); ++it){
-        rapidjson::Document polygon(rapidjson::kObjectType);
-        if (!it->toJson(polygon, allocator))
-            return false;
-        polygons.PushBack(polygon, allocator);
-    }
-    doc.AddMember("polygons", polygons, allocator);
-
-    rapidjson::Value labels(rapidjson::kArrayType);
-
-    for(auto it = this->labels.begin(); it != this->labels.end(); ++it)
-        labels.PushBack(rapidjson::Value().SetUint(*it), allocator);
-    doc.AddMember("labels", labels, allocator);
-
-    rapidjson::Value likelihood(rapidjson::kArrayType);
-
-    for(auto it = this->likelihood.begin(); it != this->likelihood.end(); ++it)
-        likelihood.PushBack(rapidjson::Value().SetFloat(*it), allocator);
-    doc.AddMember("likelihood", likelihood, allocator);
-
-    return true;
-}