json improved, topic and type issue pending

qgroundcontrol.pro

@@ -32,7 +32,7 @@ DebugBuild {
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
-    #DEFINES += DEBUG
+    DEFINES += DEBUG
     DEFINES += NDEBUG
 }
 
@@ -494,7 +494,7 @@ HEADERS += \
     src/comm/ros_bridge/include/messages/nemo_msgs/progress.h \
     src/comm/ros_bridge/include/messages/std_msgs/header.h \
     src/comm/ros_bridge/include/server.h \
-    src/comm/ros_bridge/include/time.h \
+    src/comm/ros_bridge/include/messages/std_msgs/time.h \
     src/comm/ros_bridge/include/topic_publisher.h \
     src/comm/ros_bridge/include/topic_subscriber.h \
     src/comm/utilities.h
@@ -516,7 +516,15 @@ 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/geographic_msgs/geopoint.cpp \
+    src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp \
+    src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp \
+    src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp \
+    src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp \
+    src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \
+    src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp \
     src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
+    src/comm/ros_bridge/include/messages/std_msgs/time.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/Snake/QNemoProgress.h

@@ -4,5 +4,5 @@
 
 #include "ros_bridge/include/messages/nemo_msgs/progress.h"
 
-namespace nemo_msgs = ros_bridge::messages::nemo_msgs;
-typedef nemo_msgs::progress::GenericProgress<int, QVector> QNemoProgress;
+namespace nemo = ros_bridge::messages::nemo_msgs;
+typedef nemo::progress::GenericProgress<int, QVector> QNemoProgress;

src/Wima/WimaController.cc

@@ -554,8 +554,17 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
             && _pRosBridge->isRunning()
             && _pRosBridge->connected() ){
         if ( _snakeTilesLocal.polygons().size() > 0 ){
-            _pRosBridge->publish(_snakeOrigin, "/snake/origin");
-            _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
+            using namespace ros_bridge::messages;
+            // Publish snake origin.
+            JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            Q_ASSERT(geographic_msgs::geo_point::toJson(
+                         _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+            _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+            // Publish snake tiles.
+            JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
+                         _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+            _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
         }
     }
 
@@ -944,12 +953,12 @@ void WimaController::_setupTopicService()
     using namespace ros_bridge::messages;
     if ( _snakeTilesLocal.polygons().size() > 0){
         // Publish snake origin.
-        JsonDocUPtr jOrigin(rapidjson::kObjectType);
+        JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         Q_ASSERT(geographic_msgs::geo_point::toJson(
                      _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
         _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
         // Publish snake tiles.
-        JsonDocUPtr jSnakeTiles(rapidjson::kObjectType);
+        JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
                      _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
         _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
@@ -966,12 +975,12 @@ void WimaController::_setupTopicService()
             // Publish origin and tiles again, if valid.
             if ( this->_snakeTilesLocal.polygons().size() > 0){
                 // Publish snake origin.
-                JsonDocUPtr jOrigin(rapidjson::kObjectType);
+                JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
                 Q_ASSERT(geographic_msgs::geo_point::toJson(
                              this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
                 this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
                 // Publish snake tiles.
-                JsonDocUPtr jSnakeTiles(rapidjson::kObjectType);
+                JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
                 Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
                              this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
                 this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
@@ -983,7 +992,7 @@ void WimaController::_setupTopicService()
     _pRosBridge->subscribe("/nemo/heartbeat",
                            /* callback */ [this](JsonDocUPtr pDoc){
         auto &heartbeat_msg = this->_nemoHeartbeat;
-        if ( !nemo_msgs::heartbeat(*pDoc, heartbeat_msg) ) {
+        if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeat_msg) ) {
             if ( heartbeat_msg.status() == this->_fallbackStatus )
                 return;
             heartbeat_msg.setStatus(this->_fallbackStatus);
@@ -998,13 +1007,14 @@ void WimaController::_setupTopicService()
     _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
-        JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType));
+        using namespace ros_bridge::messages;
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         rapidjson::Value jOrigin(rapidjson::kObjectType);
         if ( this->_snakeTilesLocal.polygons().size() > 0){
-            geometry_msg::geo_point::toJson(
+            geographic_msgs::geo_point::toJson(
                         this->_snakeOrigin, jOrigin, pDoc->GetAllocator());
         } else {
-            geometry_msg::geo_point::polygon_array::toJson(
+            geographic_msgs::geo_point::toJson(
                         ::GeoPoint3D(0,0,0), jOrigin, pDoc->GetAllocator());
         }
         pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
@@ -1014,7 +1024,7 @@ void WimaController::_setupTopicService()
     _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
-        JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType));
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
         jsk_recognition_msgs::polygon_array::toJson(
                     this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());

src/comm/ros_bridge/include/com_private.cpp

 #include "ros_bridge/include/com_private.h"
+
+#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
+
 #include <functional>
+#include <iostream>
 
 std::size_t ros_bridge::com_private::getHash(const std::string &str)
 {
@@ -15,7 +19,19 @@ std::size_t ros_bridge::com_private::getHash(const char *str)
 bool ros_bridge::com_private::getTopic(const ros_bridge::com_private::JsonDoc &doc, std::string &topic)
 {
     if ( doc.HasMember("topic") && doc["topic"].IsString() ){
-        topic = doc["topic"].GetString();
+
+
+        rapidjson::StringBuffer sb1;
+        rapidjson::Writer<rapidjson::StringBuffer> writer1(sb1);
+        doc.Accept(writer1);
+        std::cout << "getTopic doc: " << sb1.GetString() << std::endl;
+
+        rapidjson::StringBuffer sb;
+        rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
+        doc["topic"].Accept(writer);
+        topic = sb.GetString();
+
+        std::cout << "getTopic topic: " << sb.GetString() << std::endl;
         return true;
     } else
         return false;
@@ -33,7 +49,10 @@ bool ros_bridge::com_private::removeTopic(ros_bridge::com_private::JsonDoc &doc)
 bool ros_bridge::com_private::getType(const ros_bridge::com_private::JsonDoc &doc, std::string &type)
 {
     if ( doc.HasMember("type") && doc["type"].IsString() ){
-        type = doc["type"].GetString();
+        rapidjson::StringBuffer sb;
+        rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
+        doc["type"].Accept(writer);
+        type = sb.GetString();
         return true;
     } else
         return false;

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

+#include "geopoint.h"
+
+std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){
+    return "geometry_msgs/GeoPoint";
+}

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

@@ -13,9 +13,7 @@ namespace geographic_msgs {
 //! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
 namespace geo_point {
 
-std::string messageType(){
-    return "geometry_msgs/GeoPoint";
-}
+std::string messageType();
 
 //! @brief C++ representation of geographic_msgs/GeoPoint.
 template<class FloatType = _Float64, class OStream = std::ostream>

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

+#include "point32.h"
+
+std::string ros_bridge::messages::geometry_msgs::point32::messageType(){
+    return "geometry_msgs/Point32";
+}
+

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

@@ -12,9 +12,7 @@ namespace geometry_msgs {
 namespace point32 {
 
 
-std::string messageType(){
-    return "geometry_msgs/Point32";
-}
+std::string messageType();
 
 namespace detail {
 using namespace ros_bridge::traits;

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

+#include "polygon.h"
+
+std::string ros_bridge::messages::geometry_msgs::polygon::messageType(){
+    return "geometry_msgs/Polygon";
+}
+

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

@@ -14,9 +14,7 @@ namespace geometry_msgs {
 namespace polygon {
 
 
-std::string messageType(){
-    return "geometry_msgs/Polygon";
-}
+std::string messageType();
 
 //! @brief C++ representation of geometry_msgs/Polygon
 template <class PointTypeCVR,

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

+#include "polygon_stamped.h"
+
+std::string ros_bridge::messages::geometry_msgs::polygon_stamped::messageType(){
+    return "geometry_msgs/PolygonStamped";
+}
+

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

@@ -16,9 +16,7 @@ namespace geometry_msgs {
 namespace polygon_stamped {
 
 
-std::string messageType(){
-    return "geometry_msgs/PolygonStamped";
-}
+std::string messageType();
 
 //! @brief C++ representation of geometry_msgs/PolygonStamped
 template <class PolygonType  = geometry_msgs::polygon::Polygon,

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

+#include "polygon_array.h"
+
+std::string ros_bridge::messages::jsk_recognition_msgs::polygon_array::messageType(){
+    return "jsk_recognition_msgs/PolygonArray";
+}
+

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

@@ -16,9 +16,7 @@ namespace jsk_recognition_msgs {
 namespace polygon_array {
 
 
-std::string messageType(){
-    return "jsk_recognition_msgs/PolygonArray";
-}
+std::string messageType();
 
 
 //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
@@ -180,10 +178,10 @@ namespace detail {
         value.AddMember("labels", jLabels, allocator);
 
 
-        rapidjson::Value JLikelihood(rapidjson::kArrayType);
+        rapidjson::Value jLikelihood(rapidjson::kArrayType);
         typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-        detail::likelihoodToJson(p, JLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
-        value.AddMember("likelihood", JLikelihood, allocator);
+        detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
+        value.AddMember("likelihood", jLikelihood, allocator);
 
         rapidjson::Value jType;
         jType.SetString(messageType().c_str(), allocator);
@@ -248,13 +246,13 @@ bool toJson(const PolygonArrayType &p,
 //! will be replaced by arrays filled with zero and size p.polygons.size().
 //!
 //! \note If the header() function is missing, a default constructed header is used.
-template <class PolygonArrayType, class HeaderType>
+template <class PolygonArrayType>
 bool toJson(const PolygonArrayType &p,
             rapidjson::Value &value,
             rapidjson::Document::AllocatorType &allocator)
 {
     typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
-    return detail::_toJson(p, value, allocator, Int2Type<HasHeader::value>());
+    return detail::_toJson(p, value, allocator, traits::Int2Type<HasHeader::value>());
 }
 
 

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

+#include "heartbeat.h"
+
+std::string ros_bridge::messages::nemo_msgs::heartbeat::messageType(){
+    return "nemo_msgs/Heartbeat";
+}
+

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

@@ -11,9 +11,7 @@ namespace nemo_msgs {
 namespace heartbeat {
 
 
-std::string messageType(){
-    return "nemo_msgs/Heartbeat";
-}
+std::string messageType();
 
 
 //! @brief C++ representation of nemo_msgs/Heartbeat message

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

+#include "progress.h"
+
+std::string ros_bridge::messages::nemo_msgs::progress::messageType(){
+    return "nemo_msgs/Progress";
+}
+

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

@@ -14,9 +14,7 @@ namespace nemo_msgs {
 namespace progress {
 
 
-std::string messageType(){
-    return "nemo_msgs/Progress";
-}
+std::string messageType();
 
 //! @brief C++ representation of nemo_msgs/Progress message
 template <class IntType = long, template <class, class...> class ContainterType = std::vector>

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

@@ -63,3 +63,7 @@ void ros_bridge::messages::std_msgs::header::Header::setFrameId(
 {
     _frameId = frameId;
 }
+
+std::string ros_bridge::messages::std_msgs::header::messageType(){
+    return "std_msgs/Header";
+}

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

@@ -14,9 +14,7 @@ namespace std_msgs {
 namespace header {
 
 
-std::string messageType(){
-    return "std_msgs/Header";
-}
+std::string messageType();
 
 //! @brief C++ representation of std_msgs/Header
 class Header{

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

+#include "time.h"
+
+std::string ros_bridge::messages::std_msgs::time::messageType(){
+    return "std_msgs/Time";
+}

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

@@ -10,9 +10,7 @@ namespace std_msgs {
 //! @brief Namespace containing classes and methodes for std_msgs/Time message generation.
 namespace time {
 
-std::string messageType(){
-    return "std_msgs/Time";
-}
+std::string messageType();
 
 //! @brief C++ representation of std_msgs/Time
 class Time{

src/comm/ros_bridge/include/topic_publisher.cpp

@@ -95,9 +95,11 @@ void ros_bridge::com_private::TopicPublisher::publish(
         ros_bridge::com_private::JsonDocUPtr docPtr,
         const char *topic)
 {
-    rapidjson::Value jTopic;
-    jTopic.SetString(topic, docPtr->GetAllocator());
-    docPtr->AddMember("topic", jTopic, docPtr->GetAllocator());
+    std::cout << "TopicPublisher \"topic\": " << topic << std::endl;
+    auto &allocator = docPtr->GetAllocator();
+    rapidjson::Value key("topic", allocator);
+    rapidjson::Value value(topic, allocator);
+    docPtr->AddMember(key, value, allocator);
     std::unique_lock<std::mutex> lock(_mutex);
     _queue.push_back(std::move(docPtr));
     lock.unlock();