123

qgroundcontrol.pro

@@ -473,12 +473,13 @@ HEADERS += \
     src/Wima/testplanimetrycalculus.h \
     src/Settings/WimaSettings.h \
     src/QmlControls/QmlObjectVectorModel.h \
+    src/comm/ros_bridge/include/CasePacker.h \
     src/comm/ros_bridge/include/GenericMessages.h \
     src/comm/ros_bridge/include/JsonMethodes.h \
+    src/comm/ros_bridge/include/MessageTag.h \
     src/comm/ros_bridge/include/MessageTraits.h \
-    src/comm/ros_bridge/include/PackageBuffer.h \
+    src/comm/ros_bridge/include/PackageQueue.h \
     src/comm/ros_bridge/include/TypeFactory.h \
-    src/comm/ros_bridge/src/CasePacker.h \
     src/comm/ros_bridge/src/PackageBuffer.h \
     src/comm/utilities.h
 SOURCES += \
@@ -487,6 +488,7 @@ SOURCES += \
     src/Snake/snake_geometry.cpp \
     src/Wima/GeoPoint3D.cpp \
     src/Wima/PolygonArray.cc \
+    src/comm/ros_bridge/include/MessageTag.cpp \
     src/comm/ros_bridge/src/CasePacker.cpp \
     src/comm/ros_bridge/src/ROSCommunicator.cpp \
     src/Wima/WimaControllerDetail.cc \

src/Wima/GeoPoint3D.h

@@ -2,33 +2,33 @@
 
 #include "ros_bridge/include/JsonMethodes.h"
 #include "ros_bridge/include/MessageBaseClass.h"
-#include "ros_bridge/include/MessageGroups.h"
+#include "ros_bridge/include/GenericMessages.h"
 
 #include <QObject>
-typedef ROSBridge::MessageBaseClass<QString> ROSMsg;
-typedef ROSBridge::JsonMethodes::GeoPoint::GeoPoint ROSGeoPoint;
+typedef ROSBridge::MessageBaseClass ROSMsg;
+typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
 namespace MsgGroups = ROSBridge::MessageGroups;
-class GeoPoint3D : public QObject, public ROSMsg, public ROSGeoPoint
+class GeoPoint3D : public QObject, public ROSGeoPoint
 {
     Q_OBJECT
 public:
     typedef MsgGroups::GeoPointGroup Group;
 
     explicit GeoPoint3D(QObject *parent = nullptr)
-        : QObject(parent), ROSMsg(), ROSGeoPoint() {}
+        : QObject(parent), ROSGeoPoint() {}
     explicit GeoPoint3D(double latitude,
                         double longitude,
                         double altitude,
                         QObject *parent = nullptr)
-        : QObject(parent), ROSMsg(), ROSGeoPoint(latitude, longitude, altitude)
+        : QObject(parent), ROSGeoPoint(latitude, longitude, altitude)
     {}
     explicit GeoPoint3D(const GeoPoint3D& p,
                         QObject *parent = nullptr)
-        : QObject(parent), ROSMsg(), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
+        : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
     {}
     explicit GeoPoint3D(const ROSGeoPoint& p,
                         QObject *parent = nullptr)
-        : QObject(parent), ROSMsg(), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
+        : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
     {}
 
     virtual GeoPoint3D *Clone() const override;

src/Wima/PolygonArray.h

@@ -4,7 +4,7 @@
 
 #include "ros_bridge/include/MessageBaseClass.h"
 
-typedef ROSBridge::MessageBaseClass<QString> ROSMsgBase;
+typedef ROSBridge::MessageBaseClass ROSMsgBase;
 template <class PolygonType, template <class,class...> class ContainerType >
 class PolygonArray : public ROSMsgBase, public ContainerType<PolygonType> {
 public:

src/Wima/QtROSJsonFactory.h

@@ -2,4 +2,4 @@
 #include "ros_bridge/include/JsonFactory.h"
 #include <QString>
 
-typedef ROSBridge::JsonFactory<QString> QtROSJsonFactory;
+typedef ROSBridge::GenericJsonFactory<QString> QtROSJsonFactory;

src/Wima/WimaPolygonArray.h

@@ -7,7 +7,7 @@
 #include <QVector>
 #include <QString>
 
-typedef ROSBridge::MessageBaseClass<QString> ROSMsg;
+typedef ROSBridge::MessageBaseClass ROSMsg;
 namespace MsgGroups = ROSBridge::MessageGroups;
 typedef MsgGroups::EmptyGroup EmptyGroup;
 template <class PolygonType, template <class, class...> class ContainerType = QVector, typename GroupType = EmptyGroup>
@@ -21,7 +21,7 @@ public:
       , _polygons(other._polygons), _dirty(true)
     {}
 
-    virtual MessageBaseClass<QString> * Clone() const override{
+    virtual WimaPolygonArray *Clone() const override{
         return new WimaPolygonArray(*this);
     }
 

src/comm/ros_bridge/include/CasePacker.h

+#pragma once
+#include "ros_bridge/include/MessageBaseClass.h"
+#include "ros_bridge/include/MessageTag.h"
+#include "ros_bridge/include/TypeFactory.h"
+#include "ros_bridge/include/JsonFactory.h"
+
+#include <memory>
+#include "rapidjson/include/rapidjson/document.h"
+
+namespace ROSBridge {
+
+class CasePacker
+{
+    typedef MessageTag Tag;
+    typedef rapidjson::Document JsonDoc;
+    typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
+public:
+    CasePacker();
+
+    template<class T>
+    JsonDoc *pack(const T &msg, const std::string &topic)
+    {
+        JsonDoc *docPt(_jsonFactory.create(msg));
+        std::string messageType = T::Group::messageType();
+        _addTag(*docPt, topic, messageType.c_str());
+        return docPt;
+    }
+
+    bool showTag(const JsonDoc &doc, Tag &tag);
+
+    template<class T>
+    bool unpack(JsonDoc &doc, T &msg){
+        _removeTag(doc);
+        return _typeFactory.create(doc, msg);
+    }
+
+    static const char* topicKey;
+    static const char* messageTypeKey;
+
+protected:
+    void _addTag        (JsonDoc &doc,
+                         const std::string &topic,
+                         const std::string &messageType);
+    void _removeTag     (JsonDoc &doc);
+    bool _getTopic      (const JsonDoc &doc, std::string &topic);
+    bool _getMessageType(const JsonDoc &doc, std::string &messageType);
+
+private:
+    TypeFactory _typeFactory;
+    JsonFactory _jsonFactory;
+};
+}
+

src/comm/ros_bridge/include/JsonFactory.h

@@ -23,12 +23,12 @@ class StdHeaderPolicy;
 //! from classes derived from \class MessageBaseClass. Each class has a group mark (typedef ... Group) which allows the
 //! JsonFactory to determine the ROS message type it will create.
 template <class HeaderPolicy = StdHeaderPolicy>
-class JsonFactory : public HeaderPolicy
+class GenericJsonFactory : public HeaderPolicy
 {
     typedef MessageBaseClass ROSMsg;
 public:
 
-    JsonFactory() : HeaderPolicy() {}
+    GenericJsonFactory() : HeaderPolicy() {}
 
     //!
     //! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
@@ -127,7 +127,7 @@ private:
     template<class U>
     rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
         using namespace ROSBridge;
-        JsonMethodes::Header::Header header(HeaderPolicy::header(msg));
+        GenericMessages::StdMsgs::Header header(HeaderPolicy::header(msg));
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
         bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg.polygon(), header, *doc, doc->GetAllocator());
         assert(ret);
@@ -152,7 +152,7 @@ private:
     rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
         using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::JSKRecognitionMsg::PolygonArray::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::JSKRecognitionMsgs::PolygonArray::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -163,9 +163,9 @@ private:
     template<class U>
     rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
         using namespace ROSBridge;
-        JsonMethodes::Header::Header header(HeaderPolicy::header(msg));
+       GenericMessages::StdMsgs::Header header(HeaderPolicy::header(msg));
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::JSKRecognitionMsg::PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::JSKRecognitionMsgs::PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -187,8 +187,10 @@ private:
     }
 };
 
-class StdHeaderPolicy{
-    namespace StdMsgs = ROSBridge::GenericMessages::StdMsgs;
+class StdHeaderPolicy
+{
+    typedef ROSBridge::GenericMessages::StdMsgs::Header Header;
+    typedef ROSBridge::GenericMessages::StdMsgs::Time Time;
 public:
     StdHeaderPolicy():_seq(-1){};
 
@@ -196,24 +198,25 @@ public:
     //! \brief header Returns the header belonging to msg.
     //! \return Returns the header belonging to msg.
     //!
-    template<typename T>
-        StdMsgs::Header header(const T&msg) {
-        return StdMsgs::Header(++_seq, time(msg), "/map");
+    Header header(const ROSBridge::MessageBaseClass &msg) {
+        return Header(++_seq, time(msg), "/map");
     }
 
 
     //!
     //! \brief time Returns the current time.
     //! \return Returns the current time.
-    template<typename T>
-    StdMsgs::Time time(const T&msg) {
+    Time time(const ROSBridge::MessageBaseClass &msg) {
         (void)msg;
-        return StdMsgs::Time(0,0);
+        return Time(0,0);
     }
+
 private:
     long _seq;
 };
 
+typedef GenericJsonFactory<> JsonFactory;
+
 } // end namespace ros_bridge
 
 

src/comm/ros_bridge/include/JsonMethodes.h

@@ -636,7 +636,7 @@ namespace Progress {
             progressJson.PushBack(rapidjson::Value().SetInt(std::int8_t(p.progress()[i])), allocator);
         }
         value.AddMember("progress", progressJson, allocator);
-
+        return true;
     }
 
     template <class ProgressType>

src/comm/ros_bridge/include/MessageGroups.h

@@ -10,7 +10,7 @@ typedef std::string StringType;
 
 template<typename Group, typename SubGroup, typename...MoreSubGroups>
 struct MessageGroup {
-    static StringType messageNameFull() {return _full<Group, SubGroup, MoreSubGroups...>();}
+    static StringType messageType() {return _full<Group, SubGroup, MoreSubGroups...>();}
 
     template<typename G, typename SubG, typename...MoreSubG>
     static StringType _full() {return G::label()+ "/" + _full<SubG, MoreSubG...>(); }
@@ -18,7 +18,7 @@ struct MessageGroup {
     static StringType _full() {return G::label(); }
 
 
-    static StringType messageNameLast() {return _last<Group, SubGroup, MoreSubGroups...>();}
+    static StringType messageTypeLast() {return _last<Group, SubGroup, MoreSubGroups...>();}
 
     template<typename G, typename SubG, typename...MoreSubG>
     static StringType _last() {return _last<SubG, MoreSubG...>(); }

src/comm/ros_bridge/include/MessageTag.cpp

+#include "MessageTag.h"
+
+MessageTag::MessageTag()
+{
+
+}
+
+MessageTag::MessageTag(const std::string &topic, const std::string &messageType) :
+    _topic(topic)
+  , _messagType(messageType)
+{
+
+}
+
+const std::string &MessageTag::topic() const
+{
+    return _topic;
+}
+
+const std::string &MessageTag::messageType() const
+{
+    return _messagType;
+}
+
+std::string &MessageTag::topic()
+{
+    return _topic;
+}
+
+std::string &MessageTag::messageType()
+{
+    return _messagType;
+}
+
+void MessageTag::setTopic(const std::string &topic)
+{
+    _topic = topic;
+}
+
+void MessageTag::setMessageType(const std::string &messageType)
+{
+    _messagType = messageType;
+}

src/comm/ros_bridge/include/MessageTag.h

+#pragma once
+
+#include <string>
+
+
+class MessageTag {
+public:
+    MessageTag();
+    MessageTag(const std::string &topic, const std::string &messageType);
+
+    const std::string &topic() const;
+    const std::string &messageType() const;
+
+    std::string &topic();
+    std::string &messageType();
+
+    void setTopic(const std::string &topic);
+    void setMessageType(const std::string &messageType);
+
+private:
+   std::string _topic;
+   std::string _messagType;
+};
+typedef MessageTag Tag;

src/comm/ros_bridge/include/PackageBuffer.h → src/comm/ros_bridge/include/PackageQueue.h

@@ -5,13 +5,12 @@
 #include <functional>
 
 namespace ROSBridge {
-namespace Bridge {
 namespace lf = ::boost::lockfree;
 template <class T>
-class PackageBuffer
+class PackageQueue
 {
 public:
-    PackageBuffer();
+    PackageQueue();
 
     void push(T t) {
         buffer.push(t);
@@ -41,7 +40,5 @@ private:
     std::function<void(void)> _pushCallback;
 };
 
-} // namespace Communicator
-
 } // namespace
 

src/comm/ros_bridge/include/ROSCommunicator.h

 #pragma once
 
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/include/PackageQueue.h"
+#include "ros_bridge/include/CasePacker.h"
 
 #include <memory>
 #include <tuple>
 
 #include "boost/lockfree/queue.hpp"
-#include "ros_bridge/include/
 
 namespace ROSBridge {
 namespace lf = ::boost::lockfree;
-class Communicator
+class Communicator : protected CasePacker
 {
-    typedef std::unique_ptr<rapidjson::Document> UniqueJsonPtr;
-    typedef std::tuple<UniqueJsonPtr, std::string> MsgTopicHashPair;
+    typedef MessageTag Tag;
+    typedef rapidjson::Document JsonDoc;
+    typedef std::shared_ptr<JsonDoc> JsonDocShardPtr;
+    typedef PackageQueue<JsonDocShardPtr> Buffer;
+    //typedef std::tuple<UniqueJsonPtr, std::string> MsgTopicHashPair;
 public:
-  explicit Communicator() {}
+  explicit Communicator() : CasePacker() {}
+
+    template<class T>
+    void send(T &msg, std::string &topic){
+        JsonDocShardPtr docPtr(CasePacker::pack(msg, topic));
+        _transmittBuffer.push(docPtr);
+    }
 
-    void send(UniqueJsonPtr &msg);
     void start();
     void stop();
-    virtual UniqueJsonPtr receive() = 0;
+    bool messagesAvailable();
+    bool showMessageTag(Tag &tag);
+    template<class T>
+    void receive(T &msg){
+
+    }
 
 private:
-    lf::queue<MsgTopicPair> _transmittBuffer;
-    lf::queue<MsgTopicPair> _receiveBuffer;
+    Buffer              _transmittBuffer;
+    Buffer              _receiveBuffer;
+    JsonDocShardPtr     _stagedMessage; // message awaiting delivery
 };
 }

src/comm/ros_bridge/src/CasePacker.cpp

-#include "CasePacker.h"
+#include "ros_bridge/include/CasePacker.h"
 
 const char* ROSBridge::CasePacker::topicKey         = "topic";
 const char* ROSBridge::CasePacker::messageTypeKey   = "messageType";
@@ -8,7 +8,16 @@ ROSBridge::CasePacker::CasePacker()
 
 }
 
-void ROSBridge::CasePacker::_addTag(JsonDoc &doc, const char *topic, const char *messageType)
+bool ROSBridge::CasePacker::showTag(const ROSBridge::CasePacker::JsonDoc &doc, Tag &tag)
+{
+    if( !_getTopic(doc, tag.topic()) )
+        return false;
+    if( !_getMessageType(doc, tag.messageType()) )
+        return false;
+    return true;
+}
+
+void ROSBridge::CasePacker::_addTag(JsonDoc &doc, const std::string &topic, const std::string &messageType)
 {
     using namespace ROSBridge;
     using namespace rapidjson;
@@ -16,13 +25,13 @@ void ROSBridge::CasePacker::_addTag(JsonDoc &doc, const char *topic, const char
     {
     // add topic
     rapidjson::Value key(CasePacker::topicKey, doc.GetAllocator());
-    rapidjson::Value value(topic, doc.GetAllocator());
+    rapidjson::Value value(topic.c_str(), doc.GetAllocator());
     doc.AddMember(key, value, doc.GetAllocator());
     }
 
     // add messageType
     rapidjson::Value key(CasePacker::messageTypeKey, doc.GetAllocator());
-    rapidjson::Value value(messageType, doc.GetAllocator());
+    rapidjson::Value value(messageType.c_str(), doc.GetAllocator());
     doc.AddMember(key, value, doc.GetAllocator());
 }
 
@@ -36,4 +45,18 @@ void ROSBridge::CasePacker::_removeTag(JsonDoc &doc)
         doc.RemoveMember(CasePacker::messageTypeKey);
 }
 
+bool ROSBridge::CasePacker::_getTopic(const ROSBridge::CasePacker::JsonDoc &doc, std::string &topic)
+{
+    if (!doc.HasMember(CasePacker::topicKey) || !doc[CasePacker::topicKey].IsString())
+        return false;
+    topic = doc[CasePacker::topicKey].GetString();
+    return true;
+}
 
+bool ROSBridge::CasePacker::_getMessageType(const ROSBridge::CasePacker::JsonDoc &doc, std::string &messageType)
+{
+    if (!doc.HasMember(CasePacker::messageTypeKey) || !doc[CasePacker::messageTypeKey].IsString())
+        return false;
+    messageType = doc[CasePacker::messageTypeKey].GetString();
+    return true;
+}

src/comm/ros_bridge/src/CasePacker.h

-#pragma once
-#include "ros_bridge/include/MessageBaseClass.h"
-
-#include <memory>
-#include "rapidjson/include/rapidjson/document.h"
-
-namespace ROSBridge {
-
-class CasePacker
-{
-    typedef rapidjson::Document JsonDoc;
-    typedef std::unique_ptr<JsonDoc> UniqueJsonPtr;
-public:
-    CasePacker();
-
-    struct MessageTag {
-       char *topic;
-       char *messagType;
-    };
-    typedef MessageTag Tag;
-
-
-    template<class T>
-    void packAndSend(const T &msg, const char *topic);
-
-    const Tag &showTag();
-
-    template<class T>
-    void unpack(T &msg);
-
-protected:
-    void _addTag(JsonDoc &doc, const char *topic, const char *messageType);
-    void _removeTag(JsonDoc &doc);
-
-    static const char* topicKey;
-    static const char* messageTypeKey;
-
-private:
-    Tag _tag;
-};
-}
-

src/comm/ros_bridge/src/ROSCommunicator.cpp

 #include "ros_bridge/include/ROSCommunicator.h"
 
-void ROSBridge::Communicator::send(ROSBridge::Communicator::UniqueJsonPtr &msg)
+bool ROSBridge::Communicator::messagesAvailable()
 {
-    if (!msg->HasMember(""))
+    return !_receiveBuffer.empty() || _stagedMessage;
+}
+
+bool ROSBridge::Communicator::showMessageTag(ROSBridge::Communicator::Tag &tag)
+{
+    if (!_stagedMessage)
+        _stagedMessage = _receiveBuffer.pop();
+    return ROSBridge::Communicator::CasePacker::showTag(*_stagedMessage.get(), tag);
 }