rosbridge_msgs toJson signatures changed to (Document &, Document::AllocatorType &)

test_rosbridge/rosbridge/main.cpp

@@ -45,10 +45,19 @@ void publisherThread(RosbridgeWsClient& rbc, const std::future<void>& futureObj)
   rosbridge_msgs::Polygon polygon(std::vector<rosbridge_msgs::Point32>({rosbridge_msgs::Point32(1,1,1),
                                                                         rosbridge_msgs::Point32(2,2,2),
                                                                         rosbridge_msgs::Point32(3,3,3)}));
+  rosbridge_msgs::Polygon polygon2(std::vector<rosbridge_msgs::Point32>({rosbridge_msgs::Point32(1,1,3),
+                                                                        rosbridge_msgs::Point32(2,2,4),
+                                                                        rosbridge_msgs::Point32(3,3,5)}));
   rosbridge_msgs::PolygonStamped polygonStamped(header, polygon);
-  rapidjson::Document doc;
-  rapidjson::Value val = polygonStamped.toJson(doc.GetAllocator());
-  val.Swap(doc);
+  rosbridge_msgs::PolygonStamped polygonStamped2(header, polygon2);
+  std::vector<rosbridge_msgs::PolygonStamped> parray({polygonStamped, polygonStamped2});
+
+  std::vector<uint32_t> labels({1,2});
+  std::vector<_Float32> likelihood({1,2});
+
+  rosbridge_msgs::PolygonArray polygonArray(header, parray, labels, likelihood);
+  rapidjson::Document doc(rapidjson::kObjectType);
+  void(polygonArray.toJson(doc, doc.GetAllocator()));
 
   // Write to stdout
   rapidjson::OStreamWrapper out(std::cout);
@@ -59,7 +68,7 @@ void publisherThread(RosbridgeWsClient& rbc, const std::future<void>& futureObj)
 
   while (futureObj.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout)
   {
-    rbc.publish("/polygon_stamped_topic", doc);
+    rbc.publish("/polygon_array_topic", doc);
     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
   }
 
@@ -76,7 +85,7 @@ int main() {
 //  rbc.advertiseService("service_advertiser", "/zservice", "std_srvs/SetBool", advertiseServiceCallback);
 
   rbc.addClient("topic_advertiser");
-  rbc.advertise("topic_advertiser", "/polygon_stamped_topic", "geometry_msgs/PolygonStamped");
+  rbc.advertise("topic_advertiser", "/polygon_array_topic", "jsk_recognition_msgs/PolygonArray");
 
 //  rbc.addClient("topic_subscriber");
 //  rbc.subscribe("topic_subscriber", "/ztopic", subscriberCallback);

test_rosbridge/rosbridge/snake.cpp

@@ -13,15 +13,14 @@ namespace rosbridge_msgs {
 
     Point32::Point32(_Float32 x, _Float32 y, _Float32 z): x(x), y(y), z(z) {}
 
-    rapidjson::Value Point32::toJson(rapidjson::Document::AllocatorType &allocator)
+    bool Point32::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
     {
-        rapidjson::Value val(rapidjson::kObjectType);
 
-        val.AddMember("x", rapidjson::Value().SetFloat(this->x), allocator);
-        val.AddMember("y", rapidjson::Value().SetFloat(this->y), allocator);
-        val.AddMember("z", rapidjson::Value().SetFloat(this->z), allocator);
+        doc.AddMember("x", rapidjson::Value().SetFloat(this->x), allocator);
+        doc.AddMember("y", rapidjson::Value().SetFloat(this->y), allocator);
+        doc.AddMember("z", rapidjson::Value().SetFloat(this->z), allocator);
 
-        return val;
+        return true;
     }
 
 
@@ -32,14 +31,12 @@ namespace rosbridge_msgs {
 
     Time::Time(uint32_t secs, uint32_t nsecs): secs(secs), nsecs(nsecs) {}
 
-    rapidjson::Value Time::toJson(rapidjson::Document::AllocatorType &allocator)
+    bool Time::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
     {
-        rapidjson::Value val(rapidjson::kObjectType);
+        doc.AddMember("secs", rapidjson::Value().SetUint(this->secs), allocator);
+        doc.AddMember("nsecs", rapidjson::Value().SetUint(this->nsecs), allocator);
 
-        val.AddMember("secs", rapidjson::Value().SetUint(this->secs), allocator);
-        val.AddMember("nsecs", rapidjson::Value().SetUint(this->nsecs), allocator);
-
-        return val;
+        return true;
     }
 
 
@@ -48,17 +45,18 @@ namespace rosbridge_msgs {
     //===================================================================
     Header::Header():  seq(0), frame_id("") {}
 
-    Header::Header(uint32_t seq, Time stamp, std::string frame_id):  seq(seq), stamp(stamp), frame_id(frame_id) {}
+    Header::Header(uint32_t seq, const Time &stamp, std::string frame_id):  seq(seq), stamp(stamp), frame_id(frame_id) {}
 
-    rapidjson::Value Header::toJson(rapidjson::Document::AllocatorType &allocator)
+    bool Header::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
     {
-        rapidjson::Value val(rapidjson::kObjectType);
-
-        val.AddMember("seq", rapidjson::Value().SetUint(this->seq), allocator);
-        val.AddMember("stamp", this->stamp.toJson(allocator), allocator);
-        val.AddMember("frame_id", rapidjson::Value().SetString(this->frame_id.data(), this->frame_id.length(), allocator), allocator);
-
-        return val;
+        doc.AddMember("seq", rapidjson::Value().SetUint(this->seq), allocator);
+        rapidjson::Document stamp(rapidjson::kObjectType);
+        if (!this->stamp.toJson(stamp, allocator))
+                return false;
+        doc.AddMember("stamp", stamp, allocator);
+        doc.AddMember("frame_id", rapidjson::Value().SetString(this->frame_id.data(), this->frame_id.length(), allocator), allocator);
+
+        return true;
     }
 
 
@@ -67,20 +65,22 @@ namespace rosbridge_msgs {
     //===================================================================
     Polygon::Polygon(){}
 
-    Polygon::Polygon(std::vector<Point32> points) : points(points) {}
+    Polygon::Polygon(const std::vector<Point32> &points) : points(points) {}
 
-    rapidjson::Value Polygon::toJson(rapidjson::Document::AllocatorType &allocator)
+    bool Polygon::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
     {
-        rapidjson::Value val(rapidjson::kObjectType);
-
         rapidjson::Value points(rapidjson::kArrayType);
 
-        for(std::vector<Point32>::iterator it = this->points.begin(); it != this->points.end(); ++it)
-            points.PushBack(it->toJson(allocator), allocator);
+        for(std::vector<Point32>::iterator it = this->points.begin(); it != this->points.end(); ++it) {
+            rapidjson::Document point(rapidjson::kObjectType);
+            if (!it->toJson(point, allocator))
+                return false;
+            points.PushBack(point, allocator);
+        }
 
-        val.AddMember("points", points, allocator);
+        doc.AddMember("points", points, allocator);
 
-        return val;
+        return true;
     }
 
 
@@ -89,16 +89,20 @@ namespace rosbridge_msgs {
     //===================================================================
     PolygonStamped::PolygonStamped() {}
 
-    PolygonStamped::PolygonStamped(Header header, Polygon polygon) : header(header), polygon(polygon){}
+    PolygonStamped::PolygonStamped(const Header &header, const Polygon &polygon) : header(header), polygon(polygon){}
 
-    rapidjson::Value PolygonStamped::toJson(rapidjson::Document::AllocatorType &allocator)
+    bool PolygonStamped::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
     {
-        rapidjson::Value val(rapidjson::kObjectType);
-
-        val.AddMember("header", this->header.toJson(allocator), allocator);
-        val.AddMember("polygon", this->polygon.toJson(allocator), allocator);
-
-        return val;
+        rapidjson::Document header(rapidjson::kObjectType);
+        if (!this->header.toJson(header, allocator))
+            return false;
+        rapidjson::Document polygon(rapidjson::kObjectType);
+        if (!this->polygon.toJson(polygon, allocator))
+            return false;
+        doc.AddMember("header", header, allocator);
+        doc.AddMember("polygon", polygon, allocator);
+
+        return true;
     }
 
 
@@ -108,37 +112,42 @@ namespace rosbridge_msgs {
     //===================================================================
     PolygonArray::PolygonArray() {}
 
-    PolygonArray::PolygonArray(Header header,
-                               std::vector<Polygon> polygons,
-                               std::vector<uint32_t> labels,
-                               std::vector<_Float32> likelihood)
+    PolygonArray::PolygonArray(const Header &header,
+                               const std::vector<PolygonStamped> &polygons,
+                               const std::vector<uint32_t> &labels,
+                               const std::vector<_Float32> &likelihood)
         : header(header), polygons(polygons), labels(labels), likelihood(likelihood) {}
 
-    rapidjson::Value PolygonArray::toJson(rapidjson::Document::AllocatorType &allocator)
+    bool PolygonArray::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
     {
-        rapidjson::Value val(rapidjson::kObjectType);
-
-        val.AddMember("header", this->header.toJson(allocator), allocator);
+        rapidjson::Document header(rapidjson::kObjectType);
+        if (!this->header.toJson(header, allocator))
+            return false;
+        doc.AddMember("header", header, allocator);
 
         rapidjson::Value polygons(rapidjson::kArrayType);
 
-        for(auto it = this->polygons.begin(); it != this->polygons.end(); ++it)
-            polygons.PushBack(it->toJson(allocator), allocator);
-        val.AddMember("polygons", polygons, allocator);
+        for(auto it = this->polygons.begin(); it != this->polygons.end(); ++it){
+            rapidjson::Document polygon(rapidjson::kObjectType);
+            if (!it->toJson(polygon, allocator))
+                return false;
+            polygons.PushBack(polygon, allocator);
+        }
+        doc.AddMember("polygons", polygons, allocator);
 
         rapidjson::Value labels(rapidjson::kArrayType);
 
         for(auto it = this->labels.begin(); it != this->labels.end(); ++it)
             labels.PushBack(rapidjson::Value().SetUint(*it), allocator);
-        val.AddMember("labels", labels, allocator);
+        doc.AddMember("labels", labels, allocator);
 
-        rapidjson::Value likelyhood(rapidjson::kArrayType);
+        rapidjson::Value likelihood(rapidjson::kArrayType);
 
         for(auto it = this->likelihood.begin(); it != this->likelihood.end(); ++it)
-            likelyhood.PushBack(rapidjson::Value().SetFloat(*it), allocator);
-        val.AddMember("likelyhood", likelyhood, allocator);
+            likelihood.PushBack(rapidjson::Value().SetFloat(*it), allocator);
+        doc.AddMember("likelihood", likelihood, allocator);
 
-        return val;
+        return true;
     }
 
 }

test_rosbridge/rosbridge/snake.h

@@ -20,7 +20,7 @@ namespace rosbridge_msgs {
         Time();
         Time(uint32_t secs, uint32_t nsecs);
 
-        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator);
 
         uint32_t secs;
         uint32_t nsecs;
@@ -31,9 +31,9 @@ namespace rosbridge_msgs {
     class Header{
     public:
         Header();
-        Header(uint32_t seq, Time stamp, std::string frame_id);
+        Header(uint32_t seq, const Time &stamp, std::string frame_id);
 
-        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator);
 
         uint32_t    seq;
         Time        stamp;
@@ -47,7 +47,7 @@ namespace rosbridge_msgs {
         Point32();
         Point32(_Float32 x, _Float32 y, _Float32 z);
 
-        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator);
 
         _Float32 x;
         _Float32 y;
@@ -59,9 +59,9 @@ namespace rosbridge_msgs {
     class Polygon{
     public:
         Polygon();
-        Polygon(std::vector<Point32> points);
+        Polygon(const std::vector<Point32> &points);
 
-        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator);
 
         std::vector<Point32> points;
     };
@@ -71,9 +71,9 @@ namespace rosbridge_msgs {
     class PolygonStamped{
     public:
         PolygonStamped();
-        PolygonStamped(Header header, Polygon polygon);
+        PolygonStamped(const Header &header, const Polygon &polygon);
 
-        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator);
 
         Header header;
         Polygon polygon;
@@ -84,12 +84,12 @@ namespace rosbridge_msgs {
     class PolygonArray{
     public:
         PolygonArray();
-        PolygonArray(Header header, std::vector<Polygon> polygons, std::vector<uint32_t> labels, std::vector<_Float32> likelihood);
+        PolygonArray(const Header &header, const std::vector<PolygonStamped> &polygons, const std::vector<uint32_t> &labels, const std::vector<_Float32> &likelihood);
 
-        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+        bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator);
 
         Header header;
-        std::vector<Polygon> polygons;
+        std::vector<PolygonStamped> polygons;
         std::vector<uint32_t> labels;
         std::vector<_Float32> likelihood;
     };