Skip to content

Q
qgroundcontrol
Commit 652511a1 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

Testing ros_bridge/include/messages.h

parent 3cd1822f
Hide whitespace changes
Inline Side-by-side
Showing with 120 additions and 166 deletions
src/Wima/WimaController.cc
View file @ 652511a1
#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
View file @ 652511a1
#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
View file @ 652511a1
......@@ -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;
};
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment