polygon.h 2.79 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
#pragma once
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/include/messages/geometry_msgs/point32.h"

#include <type_traits>
#include <vector>

namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace geometry_msgs {
//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
namespace polygon {


17
std::string messageType();
18 19 20 21 22

//! @brief C++ representation of geometry_msgs/Polygon
template <class PointTypeCVR,
          template <class, class...> class ContainerType = std::vector>
class GenericPolygon {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
23
    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91
public:
    GenericPolygon() {}
    GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){}


    const ContainerType<PointType> &points() const {return _points;}
          ContainerType<PointType> &points()       {return _points;}

          GenericPolygon &operator=(const GenericPolygon &other) {
              this->_points = other._points;
              return *this;
          }

private:
    ContainerType<PointType> _points;
};
typedef GenericPolygon<geometry_msgs::point32::Point> Polygon;


template <class PolygonType>
bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
    using namespace geometry_msgs::point32;

    rapidjson::Value points(rapidjson::kArrayType);

    for(unsigned long i=0; i < std::uint64_t(poly.points().size()); ++i) {
        rapidjson::Document point(rapidjson::kObjectType);
        if ( !point32::toJson(poly.points()[i], point, allocator) ){
            assert(false);
            return false;
        }
        points.PushBack(point, allocator);
    }

    value.AddMember("points", points, allocator);
    return true;
}

template <class PolygonType>
bool fromJson(const rapidjson::Value &value, PolygonType &poly)
{
    using namespace geometry_msgs::point32;

    if (!value.HasMember("points") || !value["points"].IsArray()){
        assert(false);
        return false;
    }
    const auto &jsonArray = value["points"].GetArray();
    poly.points().clear();
    poly.points().reserve(jsonArray.Size());
    typedef decltype (poly.points()[0]) PointTypeCVR;
    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
    for (long i=0; i < jsonArray.Size(); ++i){
        PointType pt;
        if ( !point32::fromJson(jsonArray[i], pt) ){
            assert(false);
            return false;
        }
        poly.points().push_back(std::move(pt));
    }
    return true;
}

} // namespace polygon
} // namespace geometry_msgs
} // namespace messages
} // namespace ros_bridge