123

qgroundcontrol.pro

@@ -516,6 +516,7 @@ SOURCES += \
     src/Wima/WimaBridge.cc \
     src/comm/ros_bridge/include/RosBridgeClient.cpp \
     src/comm/ros_bridge/include/com_private.cpp \
+    src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
     src/comm/ros_bridge/include/server.cpp \
     src/comm/ros_bridge/include/topic_publisher.cpp \
     src/comm/ros_bridge/include/topic_subscriber.cpp \

src/Wima/Geometry/GeoPoint3D.cpp

@@ -19,11 +19,6 @@ GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D *GeoPoint3D::Clone() const
-{
-    return new GeoPoint3D(*this);
-}
-
 GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
 {
     this->setLatitude(p.latitude());

src/Wima/Geometry/GeoPoint3D.h

@@ -3,14 +3,14 @@
 #include <QGeoCoordinate>
 #include <QObject>
 
+#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
 
-typedef ros_bridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
-namespace MsgGroups = ros_bridge::MessageGroups;
+
+typedef ros_bridge::messages::geographic_msgs::geo_point::GeoPoint ROSGeoPoint;
 class GeoPoint3D : public QObject, public ROSGeoPoint
 {
     Q_OBJECT
 public:
-    typedef MsgGroups::GeoPointGroup Group;
 
     GeoPoint3D(QObject *parent = nullptr);
     GeoPoint3D(double latitude,
@@ -24,7 +24,6 @@ public:
     GeoPoint3D(const QGeoCoordinate& p,
                QObject *parent = nullptr);
 
-    virtual GeoPoint3D *Clone() const override;
     GeoPoint3D &operator=(const GeoPoint3D&p);
     GeoPoint3D &operator=(const QGeoCoordinate&p);
 

src/Wima/Geometry/Polygon2D.h

@@ -3,41 +3,30 @@
 #include <QPolygonF>
 #include <QPointF>
 
-#include "ros_bridge/include/MessageGroups.h"
-#include "ros_bridge/include/GenericMessages.h"
-#include "ros_bridge/include/MessageBaseClass.h"
-#include "ros_bridge/include/JsonMethodes.h"
+#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
 
-namespace MsgGroupsNS = ros_bridge::MessageGroups;
-namespace PolyStampedNS = ros_bridge::JsonMethodes::GeometryMsgs::PolygonStamped;
-
-typedef ros_bridge::MessageBaseClass ROSMsg;
+namespace polygon_stamped = ros_bridge::messages::geometry_msgs::polygon_stamped;
 
 template <class PointType = QPointF, template <class, class...> class ContainerType = QVector>
-class Polygon2DTemplate : public ROSMsg{ //
-    typedef ros_bridge::GenericMessages::GeometryMsgs::GenericPolygon<PointType, ContainerType> Poly;
+class Polygon2DTemplate{ //
+    typedef ros_bridge::messages::geometry_msgs::polygon::GenericPolygon<PointType, ContainerType> Polygon;
 public:
-    typedef MsgGroupsNS::PolygonStampedGroup Group; // has no header
-
     Polygon2DTemplate(){}
-    Polygon2DTemplate(const Polygon2DTemplate &other) : ROSMsg(), _polygon(other._polygon){}
+    Polygon2DTemplate(const Polygon2DTemplate &other) : _polygon(other._polygon){}
 
     Polygon2DTemplate& operator=(const Polygon2DTemplate& other) {
         this->_polygon = other._polygon;
         return *this;
     }
 
-    const Poly &polygon() const {return _polygon;}
-          Poly &polygon()       {return _polygon;}
+    const Polygon &polygon() const {return _polygon;}
+          Polygon &polygon()       {return _polygon;}
 
   const ContainerType<PointType> &path() const {return _polygon.points();}
         ContainerType<PointType> &path()       {return _polygon.points();}
 
-    virtual Polygon2DTemplate*Clone() const { // ROSMsg
-        return new Polygon2DTemplate(*this);
-    }
 private:
-    Poly _polygon;
+    Polygon _polygon;
 
 };
 

src/Wima/Geometry/PolygonArray.h

 #pragma once
 
-#include <QObject>
+#include <QString>
 
-#include "ros_bridge/include/MessageBaseClass.h"
-
-typedef ros_bridge::MessageBaseClass ROSMsgBase;
 template <class PolygonType, template <class,class...> class ContainerType >
-class PolygonArray : public ROSMsgBase, public ContainerType<PolygonType> {
+class PolygonArray : public ContainerType<PolygonType> {
 public:
-    explicit PolygonArray() : ROSMsgBase(), ContainerType<PolygonType>() {}
+    explicit PolygonArray() : ContainerType<PolygonType>() {}
     PolygonArray(const PolygonArray &other) : ContainerType<PolygonType>(other) {}
 
     QString type() const override {return "PolygonArray";}

src/Wima/Snake/SnakeTilesLocal.h

 #pragma once
 
-#include "ros_bridge/include/MessageGroups.h"
 #include "Wima/Geometry/Polygon2D.h"
 #include "Wima/Geometry/WimaPolygonArray.h"
 

src/Wima/WimaController.cc

@@ -10,8 +10,6 @@
 #include "ros_bridge/include/messages/nemo_msgs/progress.h"
 #include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
 
-#include "Snake/QtROSJsonFactory.h"
-#include "Snake/QtROSTypeFactory.h"
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
 

src/Wima/WimaController.h

@@ -40,6 +40,8 @@
 #include "WaypointManager/DefaultManager.h"
 #include "WaypointManager/RTLManager.h"
 
+#include "utilities.h"
+
 #include <map>
 
 
@@ -343,7 +345,6 @@ private slots:
 
 private:
     using StatusMap = std::map<int, QString>;
-    using CasePacker = ros_bridge::CasePacker;
 
     // Controllers.
     PlanMasterController   *_masterController;

src/comm/ros_bridge/include/com_private.h

 #pragma once
 
-#include "ros_bridge/include/MessageTag.h"
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 
 #include <memory>
@@ -10,7 +9,6 @@
 namespace ros_bridge {
 namespace com_private {
 
-typedef MessageTag Tag;
 typedef rapidjson::Document JsonDoc;
 typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
 typedef std::deque<JsonDocUPtr> JsonQueue;

src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h

@@ -111,7 +111,9 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     auto altitude = detail::getAltitude(p, traits::Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
 
     value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/point32.h

@@ -105,7 +105,9 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     auto z = detail::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
 
     value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h

@@ -22,7 +22,7 @@ std::string messageType(){
 template <class PointTypeCVR,
           template <class, class...> class ContainerType = std::vector>
 class GenericPolygon {
-    typedef typename std::remove_cv_ref_t<PointTypeCVR> PointType;
+    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
 public:
     GenericPolygon() {}
     GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){}
@@ -59,8 +59,9 @@ bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Documen
     }
 
     value.AddMember("points", points, allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
-
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h

@@ -57,12 +57,12 @@ template <class PolygonStampedType>
 bool setHeader(const rapidjson::Value &doc,
                PolygonStampedType &polyStamped,
                traits::Int2Type<1>) { // polyStamped.header() exists
-    using namespace std_msgs::header;
+    using namespace std_msgs;
 
     typedef decltype (polyStamped.header()) HeaderTypeCVR;
     typedef typename std::remove_cv_t<typename std::remove_reference_t<HeaderTypeCVR>> HeaderType;
     HeaderType header;
-    bool ret = Header::fromJson(doc, header);
+    bool ret = header::fromJson(doc, header);
     polyStamped.header() = header;
     return ret;
 }
@@ -82,8 +82,8 @@ bool _toJson(const PolygonType &poly,
              rapidjson::Value &value,
              rapidjson::Document::AllocatorType &allocator)
 {
-    using namespace std_msgs::header;
-    using namespace geometry_msgs::polygon;
+    using namespace std_msgs;
+    using namespace geometry_msgs;
 
     rapidjson::Document header(rapidjson::kObjectType);
     if (!header::toJson(h, header, allocator)){
@@ -97,7 +97,9 @@ bool _toJson(const PolygonType &poly,
     }
     value.AddMember("header", header, allocator);
     value.AddMember("polygon", polygon, allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h

@@ -152,8 +152,8 @@ namespace detail {
                  rapidjson::Value &value,
                  rapidjson::Document::AllocatorType &allocator)
     {
-        using namespace std_msgs::header;
-        using namespace geometry_msgs::polygon_stamped;
+        using namespace std_msgs;
+        using namespace geometry_msgs;
 
         rapidjson::Document jHeader(rapidjson::kObjectType);
         if (!header::toJson(h, jHeader, allocator)){
@@ -185,7 +185,9 @@ namespace detail {
         detail::likelihoodToJson(p, JLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
         value.AddMember("likelihood", JLikelihood, allocator);
 
-        value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+        rapidjson::Value jType;
+        jType.SetString(messageType().c_str(), allocator);
+        value.AddMember("type", jType, allocator);
 
         return true;
     }

src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h

@@ -34,7 +34,9 @@ template <class HeartbeatType>
 bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
 {
     value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/nemo_msgs/progress.h

@@ -43,7 +43,9 @@ bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document:
         jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
     }
     value.AddMember("progress", jProgress, allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/std_msgs/header.cpp

+#include "header.h"
+
+std::uint32_t ros_bridge::messages::std_msgs::header::Header::_defaultSeq = 0;
+
+ros_bridge::messages::std_msgs::header::Header::Header()
+    : _seq(_defaultSeq++),
+      _stamp(Time()),
+      _frameId("")
+{}
+
+ros_bridge::messages::std_msgs::header::Header::Header(
+        uint32_t seq,
+        const ros_bridge::messages::std_msgs::header::Header::Time &stamp,
+        const std::string &frame_id)
+    : _seq(seq)
+    , _stamp(stamp)
+    , _frameId(frame_id) {}
+
+ros_bridge::messages::std_msgs::header::Header::Header(
+        const ros_bridge::messages::std_msgs::header::Header &header)
+    : _seq(header.seq())
+    , _stamp(header.stamp())
+    , _frameId(header.frameId()) {}
+
+uint32_t ros_bridge::messages::std_msgs::header::Header::seq() const
+{
+    return _seq;
+}
+
+const ros_bridge::messages::std_msgs::header::Header::Time &ros_bridge::messages::std_msgs::header::Header::stamp() const
+{
+    return _stamp;
+}
+
+const std::string &ros_bridge::messages::std_msgs::header::Header::frameId() const
+{
+    return _frameId;
+}
+
+ros_bridge::messages::std_msgs::header::Header::Time &ros_bridge::messages::std_msgs::header::Header::stamp()
+{
+    return _stamp;
+}
+
+std::string &ros_bridge::messages::std_msgs::header::Header::frameId()
+{
+    return _frameId;
+}
+
+void ros_bridge::messages::std_msgs::header::Header::setSeq(uint32_t seq)
+{
+    _seq = seq;
+}
+
+void ros_bridge::messages::std_msgs::header::Header::setStamp(
+        const ros_bridge::messages::std_msgs::header::Header::Time &stamp)
+{
+    _stamp = stamp;
+}
+
+void ros_bridge::messages::std_msgs::header::Header::setFrameId(
+        const std::string &frameId)
+{
+    _frameId = frameId;
+}

src/comm/ros_bridge/include/messages/std_msgs/header.h

@@ -22,29 +22,22 @@ std::string messageType(){
 class Header{
 public:
     using Time = std_msgs::time::Time;
-    Header() : _seq(_defaultSeq++), _stamp(Time()), _frameId("") {}
-    Header(uint32_t seq, const Time &stamp, const std::string &frame_id) :
-        _seq(seq)
-      , _stamp(stamp)
-      , _frameId(frame_id) {}
-
-    Header(const Header &header) :
-        _seq(header.seq())
-      , _stamp(header.stamp())
-      , _frameId(header.frameId()) {}
-
-    uint32_t            seq()       const {return _seq;}
-    const Time          &stamp()     const {return _stamp;}
-    const std::string   &frameId()   const {return _frameId;}
-
-    Time            &stamp()         {return _stamp;}
-    std::string     &frameId()       {return _frameId;}
-
-    void setSeq     (uint32_t seq)                  {_seq = seq;}
-    void setStamp   (const Time &stamp)             {_stamp = stamp;}
-    void setFrameId (const std::string  &frameId)   {_frameId = frameId;}
+    Header();
+    Header(uint32_t seq, const Time &stamp, const std::string &frame_id);
+    Header(const Header &header);
+
+    uint32_t            seq()       const;
+    const Time          &stamp()     const;
+    const std::string   &frameId()   const;
+
+    Time            &stamp();
+    std::string     &frameId();
+
+    void setSeq     (uint32_t seq);
+    void setStamp   (const Time &stamp);
+    void setFrameId (const std::string  &frameId);
 private:
-    static uint32_t _defaultSeq = 0;
+    static uint32_t _defaultSeq;
     uint32_t    _seq;
     Time        _stamp;
     std::string _frameId;
@@ -53,11 +46,14 @@ private:
 template <class HeaderType>
 bool toJson(const HeaderType &header,
             rapidjson::Value &value,
-            rapidjson::Document::AllocatorType &allocator)     {
+            rapidjson::Document::AllocatorType &allocator)
+{
+    using namespace messages::std_msgs;
+
     value.AddMember("seq", rapidjson::Value().SetUint(uint32_t(header.seq())), allocator);
 
     rapidjson::Value stamp(rapidjson::kObjectType);
-    if (!Time::toJson(header.stamp(), stamp, allocator)){
+    if (!time::toJson(header.stamp(), stamp, allocator)){
         assert(false);
         return false;
     }
@@ -68,14 +64,19 @@ bool toJson(const HeaderType &header,
                                                header.frameId().length(),
                                                allocator),
                   allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
 
     return true;
 }
 
 template <class HeaderType>
 bool fromJson(const rapidjson::Value &value,
-              HeaderType &header)     {
+              HeaderType &header)
+{
+    using namespace messages::std_msgs;
+
     if (!value.HasMember("seq")|| !value["seq"].IsUint()){
         assert(false);
         return false;
@@ -90,7 +91,7 @@ bool fromJson(const rapidjson::Value &value,
     }
     header.setSeq(value["seq"].GetUint());
     decltype(header.stamp()) time;
-    if (!Time::fromJson(value["stamp"], time)){
+    if (!time::fromJson(value["stamp"], time)){
         assert(false);
         return false;
     }

src/comm/ros_bridge/include/messages/std_msgs/time.h

@@ -39,7 +39,9 @@ bool toJson(const TimeType &time,
 {
     value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
     value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/ros_bridge.h

 #pragma once
 
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/include/CasePacker.h"
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/JsonFactory.h"
 #include "ros_bridge/include/topic_publisher.h"
 #include "ros_bridge/include/topic_subscriber.h"
 #include "ros_bridge/include/server.h"

src/comm/ros_bridge/include/server.cpp

@@ -2,9 +2,8 @@
 
 #include "rapidjson/include/rapidjson/ostreamwrapper.h"
 
-ros_bridge::com_private::Server::Server(CasePacker &casePacker, RosbridgeWsClient &rbc) :
+ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) :
     _rbc(rbc)
-  , _casePacker(casePacker)
   , _stopped(std::make_shared<std::atomic_bool>(true))
 {
 
@@ -22,9 +21,8 @@ void ros_bridge::com_private::Server::advertiseService(const std::string &servic
     _rbc.addClient(clientName);
 
     auto rbc = &_rbc;
-    auto casePacker = &_casePacker;
     _rbc.advertiseService(clientName, service, type,
-                          [userCallback, rbc, casePacker](
+                          [userCallback, rbc](
                           std::shared_ptr<WsClient::Connection>,
                           std::shared_ptr<WsClient::InMessage> in_message){
         // message->string() is destructive, so we have to buffer it first

src/comm/ros_bridge/include/server.h

@@ -2,8 +2,6 @@
 
 #include "ros_bridge/include/com_private.h"
 #include "ros_bridge/include/RosBridgeClient.h"
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/CasePacker.h"
 
 #include <unordered_map>
 
@@ -18,7 +16,7 @@ public:
 
 
     Server() = delete;
-    Server(CasePacker &casePacker, RosbridgeWsClient &rbc);
+    Server(RosbridgeWsClient &rbc);
     ~Server();
 
     //! @brief Starts the subscriber.
@@ -34,7 +32,6 @@ public:
 private:
 
     RosbridgeWsClient  &_rbc;
-    CasePacker         &_casePacker;
     ServiceMap         _serviceMap;
     std::shared_ptr<std::atomic_bool> _stopped;
 };

src/comm/ros_bridge/include/topic_publisher.cpp

@@ -2,10 +2,8 @@
 
 #include <unordered_map>
 
-ros_bridge::com_private::TopicPublisher::TopicPublisher(CasePacker &casePacker,
-                                                      RosbridgeWsClient &rbc) :
+ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc) :
       _stopped(std::make_shared<std::atomic_bool>(true))
-    , _casePacker(casePacker)
     , _rbc(rbc)
 {
 
@@ -95,8 +93,11 @@ void ros_bridge::com_private::TopicPublisher::reset()
 
 void ros_bridge::com_private::TopicPublisher::publish(
         ros_bridge::com_private::JsonDocUPtr docPtr,
-        const char *topic){
-    docPtr->AddMember("topic", topic, doc->GetAllocator());
+        const char *topic)
+{
+    rapidjson::Value jTopic;
+    jTopic.SetString(topic, docPtr->GetAllocator());
+    docPtr->AddMember("topic", jTopic, docPtr->GetAllocator());
     std::unique_lock<std::mutex> lock(_mutex);
     _queue.push_back(std::move(docPtr));
     lock.unlock();

src/comm/ros_bridge/include/topic_publisher.h

@@ -20,8 +20,7 @@ class TopicPublisher
 public:
 
     TopicPublisher() = delete;
-    TopicPublisher(CasePacker &casePacker,
-                   RosbridgeWsClient &rbc);
+    TopicPublisher(RosbridgeWsClient &rbc);
 
     ~TopicPublisher();