ROSJsonFactory running, not fully tested.

Paths/KlingenbachSurvey.plan

-{
-    "fileType": "Plan",
-    "geoFence": {
-        "circles": [
-        ],
-        "polygons": [
-        ],
-        "version": 2
-    },
-    "groundStation": "QGroundControl",
-    "mission": {
-        "cruiseSpeed": 15,
-        "firmwareType": 3,
-        "hoverSpeed": 2,
-        "items": [
-            {
-                "autoContinue": true,
-                "command": 22,
-                "doJumpId": 1,
-                "frame": 3,
-                "params": [
-                    15,
-                    0,
-                    0,
-                    null,
-                    0,
-                    0,
-                    15
-                ],
-                "type": "SimpleItem"
-            },
-            {
-                "TransectStyleComplexItem": {
-                    "CameraCalc": {
-                        "AdjustedFootprintFrontal": 25,
-                        "AdjustedFootprintSide": 25,
-                        "CameraName": "Manual (no camera specs)",
-                        "DistanceToSurface": 16,
-                        "DistanceToSurfaceRelative": true,
-                        "version": 1
-                    },
-                    "CameraShots": 6,
-                    "CameraTriggerInTurnAround": true,
-                    "FollowTerrain": false,
-                    "HoverAndCapture": false,
-                    "Items": [
-                        {
-                            "autoContinue": true,
-                            "command": 16,
-                            "doJumpId": 2,
-                            "frame": 3,
-                            "params": [
-                                0,
-                                0,
-                                0,
-                                null,
-                                47.76823086287187,
-                                16.53058942509675,
-                                12
-                            ],
-                            "type": "SimpleItem"
-                        },
-                        {
-                            "autoContinue": true,
-                            "command": 206,
-                            "doJumpId": 3,
-                            "frame": 2,
-                            "params": [
-                                25,
-                                0,
-                                1,
-                                0,
-                                0,
-                                0,
-                                0
-                            ],
-                            "type": "SimpleItem"
-                        },
-                        {
-                            "autoContinue": true,
-                            "command": 206,
-                            "doJumpId": 4,
-                            "frame": 2,
-                            "params": [
-                                25,
-                                0,
-                                1,
-                                0,
-                                0,
-                                0,
-                                0
-                            ],
-                            "type": "SimpleItem"
-                        },
-                        {
-                            "autoContinue": true,
-                            "command": 16,
-                            "doJumpId": 5,
-                            "frame": 3,
-                            "params": [
-                                0,
-                                0,
-                                0,
-                                null,
-                                47.767777545528425,
-                                16.530589422507354,
-                                12
-                            ],
-                            "type": "SimpleItem"
-                        },
-                        {
-                            "autoContinue": true,
-                            "command": 16,
-                            "doJumpId": 6,
-                            "frame": 3,
-                            "params": [
-                                0,
-                                0,
-                                0,
-                                null,
-                                47.7678946477262,
-                                16.53092392473375,
-                                12
-                            ],
-                            "type": "SimpleItem"
-                        },
-                        {
-                            "autoContinue": true,
-                            "command": 206,
-                            "doJumpId": 7,
-                            "frame": 2,
-                            "params": [
-                                25,
-                                0,
-                                1,
-                                0,
-                                0,
-                                0,
-                                0
-                            ],
-                            "type": "SimpleItem"
-                        },
-                        {
-                            "autoContinue": true,
-                            "command": 16,
-                            "doJumpId": 8,
-                            "frame": 3,
-                            "params": [
-                                0,
-                                0,
-                                0,
-                                null,
-                                47.76834255995745,
-                                16.530923930172992,
-                                12
-                            ],
-                            "type": "SimpleItem"
-                        },
-                        {
-                            "autoContinue": true,
-                            "command": 206,
-                            "doJumpId": 9,
-                            "frame": 2,
-                            "params": [
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0,
-                                0
-                            ],
-                            "type": "SimpleItem"
-                        }
-                    ],
-                    "Refly90Degrees": false,
-                    "TurnAroundDistance": 0,
-                    "VisualTransectPoints": [
-                        [
-                            47.76823086287187,
-                            16.53058942509675
-                        ],
-                        [
-                            47.767777545528425,
-                            16.530589422507354
-                        ],
-                        [
-                            47.7678946477262,
-                            16.53092392473375
-                        ],
-                        [
-                            47.76834255995745,
-                            16.530923930172992
-                        ]
-                    ],
-                    "version": 1
-                },
-                "angle": 0,
-                "complexItemType": "survey",
-                "entryLocation": 0,
-                "flyAlternateTransects": false,
-                "polygon": [
-                    [
-                        47.76813165710231,
-                        16.530292332060014
-                    ],
-                    [
-                        47.76836780779515,
-                        16.530999541789157
-                    ],
-                    [
-                        47.767985395495465,
-                        16.531183148525685
-                    ],
-                    [
-                        47.76771877171136,
-                        16.530421536814117
-                    ]
-                ],
-                "splitConcavePolygons": false,
-                "type": "ComplexItem",
-                "version": 5
-            }
-        ],
-        "plannedHomePosition": [
-            47.7677909,
-            16.5305408,
-            0.01
-        ],
-        "vehicleType": 2,
-        "version": 2
-    },
-    "rallyPoints": {
-        "points": [
-        ],
-        "version": 2
-    },
-    "version": 1
-}

qgroundcontrol.pro

@@ -429,7 +429,19 @@ HEADERS += \
     src/Snake/snake.h \
     src/Snake/snake_geometry.h \
     src/Snake/snake_global.h \
+    src/Wima/GeoPoint3D.h \
+    src/Wima/Polygon2D.h \
+    src/Wima/PolygonArray.h \
+    src/Wima/QtROSJsonFactory.h \
+    src/Wima/ROSCommunicator.h \
+    src/Wima/ROSJsonFactory.h \
+    src/Wima/ROSMessageGroups.h \
+    src/Wima/ROSMessageType.h \
+    src/Wima/SnakeTiles.h \
+    src/Wima/SnakeTilesLocal.h \
     src/Wima/WimaControllerDetail.h \
+    src/Wima/WimaPolygonArray.h \
+    src/Wima/snaketile.h \
     src/api/QGCCorePlugin.h \
     src/api/QGCOptions.h \
     src/api/QGCSettings.h \
@@ -443,7 +455,6 @@ HEADERS += \
     src/Wima/WimaVehicle.h \
     src/Wima/WimaDataContainer.h \
     src/Wima/WimaPlaner.h \
-    src/Wima/WimaPlaner.h \
     src/Wima/WimaMeasurementArea.h \
     src/Wima/WimaCorridor.h \
     src/Wima/WimaAreaData.h \
@@ -464,14 +475,19 @@ HEADERS += \
     src/Wima/testplanimetrycalculus.h \
     src/Settings/WimaSettings.h \
     src/QmlControls/QmlObjectVectorModel.h \
-    src/comm/ros_bridge/include/jsongenerator.h \
+    src/comm/ros_bridge/include/ROSMessageTraits.h \
     src/comm/ros_bridge/include/messages.h \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
     src/Snake/snake.cpp \
     src/Snake/snake_geometry.cpp \
+    src/Wima/GeoPoint3D.cpp \
+    src/Wima/PolygonArray.cc \
+    src/Wima/ROSCommunicator.cpp \
+    src/Wima/ROSJsonFactory.cpp \
     src/Wima/WimaControllerDetail.cc \
+    src/Wima/snaketile.cpp \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \
     src/api/QGCSettings.cc \
@@ -503,8 +519,7 @@ SOURCES += \
     src/Wima/TestPolygonCalculus.cpp \
     src/Wima/testplanimetrycalculus.cpp \
     src/Settings/WimaSettings.cc \
-    src/QmlControls/QmlObjectVectorModel.cc \
-    src/comm/ros_bridge/src/messages.cpp
+    src/QmlControls/QmlObjectVectorModel.cc
 
 #
 # Unit Test specific configuration goes here (requires full debug build with all plugins)

src/Wima/GeoPoint3D.cpp

+#include "GeoPoint3D.h"
+
+GeoPoint3D *GeoPoint3D::Clone() const
+{
+    return new GeoPoint3D(*this);
+}
+
+GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
+{
+    this->setLatitude(p.latitude());
+    this->setLongitude(p.longitude());
+    this->setAltitude(p.altitude());
+
+    return *this;
+}

src/Wima/GeoPoint3D.h

+#pragma once
+
+#include "ros_bridge/include/messages.h"
+#include "ROSMessageType.h"
+#include "ROSMessageGroups.h"
+
+#include <QObject>
+typedef ROSMessageType<QString> ROSMsg;
+typedef ros_bridge::GeoPoint::GeoPoint ROSGeoPoint;
+class GeoPoint3D : public QObject, public ROSMsg, public ROSGeoPoint
+{
+    Q_OBJECT
+public:
+    typedef GeoPointGroup Group;
+
+    explicit GeoPoint3D(QObject *parent = nullptr)
+        : QObject(parent), ROSMsg(), ROSGeoPoint() {}
+    explicit GeoPoint3D(double latitude,
+                        double longitude,
+                        double altitude,
+                        QObject *parent = nullptr)
+        : QObject(parent), ROSMsg(), ROSGeoPoint(latitude, longitude, altitude)
+    {}
+    explicit GeoPoint3D(const GeoPoint3D& p,
+                        QObject *parent = nullptr)
+        : QObject(parent), ROSMsg(), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
+    {}
+    explicit GeoPoint3D(const class ros_bridge::GeoPoint::GeoPoint& p,
+                        QObject *parent = nullptr)
+        : QObject(parent), ROSMsg(), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
+    {}
+
+    virtual GeoPoint3D *Clone() const override;
+    GeoPoint3D &operator=(const GeoPoint3D&p);
+
+};

src/Wima/Polygon2D.h

+#pragma once
+
+#include <QPolygonF>
+#include <QPointF>
+
+#include "ROSMessageGroups.h"
+#include "ROSMessageType.h"
+
+typedef ROSMessageType<QString> ROSMsg;
+
+class Polygon2D : public QPolygonF, public ROSMsg{
+public:
+    typedef PolygonGroup Group;
+
+    Polygon2D(){}
+    Polygon2D(const Polygon2D &other) : QPolygonF(other) , ROSMsg(){}
+
+    Polygon2D& operator=(const Polygon2D& other) {
+        QPolygonF::operator=(other);
+        return *this;
+    }
+
+    virtual Polygon2D*Clone() const { // ROSMsg
+        return new Polygon2D(*this);
+    }
+
+    const Polygon2D &points() const {return *this;} // ROSMsg
+    Polygon2D &points() {return *this;} // ROSMsg
+
+};

src/Wima/PolygonArray.cc

+#include "PolygonArray.h"
+
+template <class PolygonType, template <class,class...> class ContainerType >
+const char *PolygonArray<PolygonType, ContainerType>::typeName = "PolygonArray";

src/Wima/PolygonArray.h

+#pragma once
+
+#include <QObject>
+
+#include "ROSMessageType.h"
+
+
+template <class PolygonType, template <class,class...> class ContainerType >
+class PolygonArray : public ROSMessageType<QString>, public ContainerType<PolygonType> {
+public:
+    explicit PolygonArray() : ROSMessageType<QString>(), ContainerType<PolygonType>() {}
+    PolygonArray(const PolygonArray &other) : ContainerType<PolygonType>(other) {}
+
+    QString type() const override {return typeName;}
+
+    static const char *typeName;
+};

src/Wima/QtROSJsonFactory.h

+#pragma once
+#include "ROSJsonFactory.h"
+
+typedef ROSJsonFactory<> QtROSJsonFactory;

src/Wima/ROSCommunicator.cpp

+#include "ROSCommunicator.h"
+
+void ROSCommunicator::send(const ROSMsg *msg)
+{
+
+}

src/Wima/ROSCommunicator.h

+#pragma once
+
+#include "WimaAreaData.h"
+#include "ROSMessageType.h"
+
+typedef ROSMessageType<QString> ROSMsg;
+
+class ROSCommunicator : public QObject
+{
+  Q_OBJECT
+
+public:
+  explicit ROSCommunicator(QObject *parent = 0) :
+      QObject(parent) {}
+
+    void send(const ROSMsg *msg);
+
+
+public slots:
+    virtual void receive(ROSMsg *msg) = 0;
+
+signals:
+  void readAsynkPolledChanged(int value);
+};

src/Wima/ROSJsonFactory.cpp

+#include "ROSJsonFactory.h"
+#include "PolygonArray.h"

src/Wima/ROSJsonFactory.h

+#ifndef ROSJSONFACTORY_H
+#define ROSJSONFACTORY_H
+
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/include/messages.h"
+#include "ROSMessageType.h"
+#include <QString>
+#include "WimaAreaData.h"
+#include "utilities.h"
+#include "ros_bridge/include/ROSMessageTraits.h"
+
+#include "ROSMessageGroups.h"
+
+#include "boost/type_traits.hpp"
+#include "boost/type_traits/is_base_of.hpp"
+
+class StdHeaderPolicy;
+
+//!
+//! \brief The ROSJsonFactory class is used to create ROS messages.
+//!
+//! The ROSJsonFactory class is used to create \class rapidjson::Document documents containing ROS messages
+//! from classes derived from \class ROSMessageType. Each class has a group mark (typedef ... Group) which allows the
+//! ROSJsonFactory to determine the ROS message type it will create.
+template <class HeaderPolicy = StdHeaderPolicy>
+class ROSJsonFactory : public HeaderPolicy
+{
+    typedef QString StringType;
+    typedef ROSMessageType<StringType> ROSMsg;
+public:
+
+    ROSJsonFactory(){}
+
+    //!
+    //! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
+    //!
+    //! Creates a \class rapidjson::Document document containing a ROS message from \p msg.
+    //! A compile-time error will occur, if \p msg belongs to the \struct EmptyGroup or is
+    //! not derived from \class ROSMessageType.
+    //! \param msg Instance of a \class ROSMessageType subclass belonging to a ROSMessageGroup.
+    //! \return rapidjson::Document document containing a ROS message.
+    template <class T>
+    rapidjson::Document *create(const T &msg){
+        static_assert(boost::is_base_of<ROSMsg, T>::value, "Type of msg must be derived from ROSMessageType.");
+        static_assert(!::boost::is_same<typename T::Group, EmptyGroup>::value,
+                "msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)");
+        return _create(msg, Type2Type<typename T::Group>());
+    }
+
+private:
+
+    // ===========================================================================
+    // EmptyGroup and not implemented Groups
+    // ===========================================================================
+    template<class U, class V>
+    rapidjson::Document *_create(const U &msg, Type2Type<V>){
+        (void)msg;
+        assert(false); // Implementation missing for group U::Group!
+    }
+
+    // ===========================================================================
+    // Point32Group
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<Point32Group>){
+        using namespace ros_bridge;
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+    }
+
+    // ===========================================================================
+    // GeoPointGroup
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<GeoPointGroup>){
+        using namespace ros_bridge;
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = GeoPoint::toJson(msg, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+    }
+
+    // ===========================================================================
+    // PolygonGroup
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<PolygonGroup>){
+        using namespace ros_bridge;
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = Polygon::toJson(msg, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+    }
+
+    // ===========================================================================
+    // PolygonStampedGroup
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<PolygonStampedGroup>){
+        using namespace ros_bridge;
+        using namespace ros_bridge::traits;
+        typedef HasMemberHeader<U> HasHeader;
+
+        return _createPolygonStamped(msg, Int2Type<HasHeader::value>());
+    }
+
+    template<class U, int k>
+    rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
+        using namespace ros_bridge;
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = PolygonStamped::toJson(msg, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+
+    }
+
+    template<class U>
+    rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
+        using namespace ros_bridge;
+        Header::Header header(HeaderPolicy::header(msg));
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = PolygonStamped::toJson(msg, header, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+    }
+
+    // ===========================================================================
+    // PolygonArrayGroup
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<PolygonArrayGroup>){
+        using namespace ros_bridge;
+        using namespace ros_bridge::traits;
+        typedef HasMemberHeader<U> HasHeader;
+
+        return _createPolygonArray(msg, Int2Type<HasHeader::value>());
+    }
+
+    template<class U, int k>
+    rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
+        using namespace ros_bridge;
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = PolygonArray::toJson(msg, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+
+    }
+
+    template<class U>
+    rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
+        using namespace ros_bridge;
+        Header::Header header(HeaderPolicy::header(msg));
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
+
+        return doc;
+    }
+
+
+};
+
+class StdHeaderPolicy{
+public:
+    StdHeaderPolicy():_seq(-1){};
+
+    //!
+    //! \brief header Returns the header belonging to msg.
+    //! \return Returns the header belonging to msg.
+    //!
+    template<typename T>
+        ros_bridge::Header::Header header(const T&msg) {
+        return ros_bridge::Header::Header(++_seq, time(msg), "/map");
+    }
+
+
+    //!
+    //! \brief time Returns the current time.
+    //! \return Returns the current time.
+    template<typename T>
+    ros_bridge::Time::Time time(const T&msg) {
+        (void)msg;
+        return ros_bridge::Time::Time(0,0);
+    }
+private:
+    long _seq;
+};
+
+#endif // ROSJSONFACTORY_H

src/Wima/ROSMessageGroups.h

+#pragma once
+
+namespace detail {
+}
+
+//!
+//! \note Each calls belonging to a ROS message group must derive from \class ROSMessageType.
+
+//!
+//! \brief The EmptyGroup struct is used by the \class ROSMessageType base class.
+//!
+//! The EmptyGroup struct is used by the \class ROSMessageType base class. Passing a class using this
+//! group will to the \class ROSJsonFactory will lead to a compile-time error.
+struct EmptyGroup        {};
+
+//!
+//! \brief The Point32Group struct is used the mark a class as a ROS Point32 message.
+//!
+//! The Point32Group struct is used the mark a class as a ROS Point32 message.
+struct Point32Group {};
+
+//!
+//! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
+//!
+//! The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
+struct GeoPointGroup {};
+
+//!
+//! \brief The PolygonGroup struct is used the mark a class as a ROS Polygon message.
+//!
+//! The PolygonGroup struct is used the mark a class as a ROS Polygon message.
+struct PolygonGroup {};
+
+//!
+//! \brief The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
+//!
+//! The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
+struct PolygonStampedGroup {};
+
+
+//!
+//! \brief The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
+//!
+//! The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
+struct PolygonArrayGroup {};

src/Wima/SnakeTiles.h

+#ifndef SNAKETILES_H
+#define SNAKETILES_H
+
+#endif // SNAKETILES_H

src/Wima/SnakeTilesLocal.h

+#ifndef SNAKETILELOCAL_H
+#define SNAKETILELOCAL_H
+
+#endif // SNAKETILELOCAL_H

src/comm/ros_bridge/include/jsongenerator.h

-#ifndef JSONGENERATOR_H
-#define JSONGENERATOR_H
-
-
-#endif // JSONGENERATOR_H