diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 1bbcb23d8e82db5e79068225093c9bfe63680109..cfc3d395ad2ec312d2856cdaa2ac8ba6866d17c9 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -1,5 +1,9 @@ #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 writer(out); + doc.Accept(writer); + std::cout << std::endl; + } } void WimaController::_smartRTLCleanUp(bool flying) diff --git a/src/comm/ros_bridge/include/jsongenerator.h b/src/comm/ros_bridge/include/jsongenerator.h index 55cc35950d4983425b5a1600c60594008c2fabba..f006ad0fdb00f6480cb49afb66e991aa72fc7450 100644 --- a/src/comm/ros_bridge/include/jsongenerator.h +++ b/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 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 diff --git a/src/comm/ros_bridge/include/messages.h b/src/comm/ros_bridge/include/messages.h index 279386c85658d9812c0ed96a5ec28ef63d565e3a..e666dfa4be2cece9f9c53eca13113f47005bbc1f 100644 --- a/src/comm/ros_bridge/include/messages.h +++ b/src/comm/ros_bridge/include/messages.h @@ -5,19 +5,28 @@ #include #include +#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 Point32{ - public: - Point32(); - Point32(_Float32 x, _Float32 y, _Float32 z); + public: + Point32(): Point32(0, 0, 0) {} + Point32(const PointType* p): _point(p) {} - template - 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 - _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 + template , + template class ContainerType = std::vector> class Polygon{ public: Polygon(){} - Polygon(const std::vector &points) : _points(points) {} - Polygon(const Polygon &poly) : _points(poly.get()) {} + Polygon(const ContainerType *points) : _points(&points) {} - const std::vector &get() const - { - return _points; - } - void set(std::vector &points) - { - _points = points; - } + const ContainerType *getPoints() const {return _points;} - void clear() - { - _points.clear(); - } + void setPoints(const ContainerType *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 _points; + const ContainerType *_points; }; //! @brief C++ representation of geometry_msgs/PolygonStamped - template > + template 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 HeaderType = Header, template class ContainerType = std::vector > + template , + template class ContainerType = std::vector > class PolygonArray{ public: - PolygonArray() - : PolygonArray(HeaderType() - , ContainerType() - , ContainerType() - , ContainerType<_Float32>()) - {} - - PolygonArray(const HeaderType &header, - const ContainerType &polygons, - const ContainerType &labels, - const ContainerType<_Float32> &likelihood) + 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 &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;} + + + private: - HeaderType _header; - ContainerType _polygons; - ContainerType _labels; - ContainerType<_Float32> _likelihood; + HeaderType _header; + std::vector _polygons; + std::vector _labels; + std::vector<_Float32> _likelihood; }; } diff --git a/src/comm/ros_bridge/src/messages.cpp b/src/comm/ros_bridge/src/messages.cpp index a93f11ebc1c3fee380027e1f7e3f905df5128b8d..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 100644 --- a/src/comm/ros_bridge/src/messages.cpp +++ b/src/comm/ros_bridge/src/messages.cpp @@ -1,143 +0,0 @@ -#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 &polygons, - const std::vector &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; -} diff --git a/test_rosbridge/rosbridge/.snake.cpp.kate-swp b/test_rosbridge/rosbridge/.snake.cpp.kate-swp new file mode 100644 index 0000000000000000000000000000000000000000..ca1900d5da57dac27c577701975e9a9e0dcb4d04 Binary files /dev/null and b/test_rosbridge/rosbridge/.snake.cpp.kate-swp differ