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;
}